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Abstract 

Waltz  is  a  typical  example  of  physical  human-human  interaction  (pHHI)  in  a 
well-structured  environment,  which  makes  waltz  a  good  start  point  for  under¬ 
standing  pHHI  and  physical  human- robot  interaction  (pHRI) .  Waltz  involves 
two  dancers.  Understanding  of  the  female  dancer’s  abilities  in  dancing  may 
help  designing  robots  that  can  physically  interact  with  human  in  intuitive 
ways.  Therefore,  our  goal  is  to  reproduce  the  female  dancer’s  abilities  with 
a  robot  in  pHRI.  We  focus  on  the  lower  level  interaction  in  pHRI,  i.e.,  cou¬ 
pled  dynamics.  We  propose  a  framework  which  covers  modeling,  analysis, 
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human  state  sensing  and  robot  control  in  developing  a  cooperative  female 
robot  dancer.  We  model  the  two  dancers’  coupled  dynamics  as  two  physi¬ 
cally  connected  inverted  pendulums.  Stability  of  this  two-pendulum  system 
is  analyzed.  The  human  state  is  measured  by  two  laser  range  sensors,  while 
the  measurement  noise  and  bias  are  corrected  by  a  Kalman  filter.  Several 
candidate  robot  controllers  are  discussed  and  evaluated  in  experiments.  Our 
contributions  include: 

1.  A  model  for  describing  dancers’  coupled  dynamics  in  waltz; 

2.  Implementation  of  poly-quadratic  stability  condition  in  proving  the 
two-LIPM  system’s  stability; 

3.  A  novel  method  which  uses  LRF  to  infer  human’s  timing  in  pHRI,  and 
a  Kalman- filter-based  method  for  estimating  the  state  of  human; 

4.  Analysis  and  validation  of  several  robot  controllers. 

1  Introduction 

Today,  a  vast  variety  of  robots  have  been  created  to  help  human.  To  realize 
this  design  goal,  robots  must  interact  with  the  external  world.  Depending 
on  the  requirements  of  tasks,  robots  may  interact  with  diversified  objects, 
including  human.  Because  of  the  extremely  complicated  nature  of  human,  a 
large  field  of  study  is  dedicated  to  understanding,  designing,  and  evaluating 
robot  systems  for  human-robot  interaction  (HRI)  [1], 

HRI  can  be  on  the  cognitive  level  (cognitive  human-robot  interaction, 
cHRI  [2])  and  physical  level  (physical  human-robot  interaction,  pHRI  [3]). 
As  in  the  near  future  human  and  robot  are  expected  to  cooperate  within  the 
shared  workspace  in  industrial  and  domestic  applications,  pHRI  is  drawing 
increasing  attention  and  has  become  one  of  the  major  focuses  in  robotics 
research. 

Because  human  has  the  strong  ability  to  cooperate  with  each  other,  we 
could  assume  that  for  the  same  task,  the  pHHI  (physical  human-human  inter¬ 
action)  may  have  superior  performance  than  the  pHRI.  Besides  performance, 
we  may  have  more  “natural”  user  experience  in  pHHI  than  in  pHRI.  Due  to 
the  above  reasons,  understanding  the  pHHI  could  help  the  researchers  to 
create  more  cooperative  robots  to  enhance  the  pHRI  performance  [4], 

Among  the  millions  of  pHHI,  we  select  waltz  as  the  subject  of  our  study. 
One  reason  is  that  waltz  is  a  real-world,  interesting  pHHI;  a  waltz  partner 
robot  which  has  desired  pHRI  performance  has  the  potential  to  be  socially 
adopted  for  entertainment  and  rehabilitation  purposes  [5].  Another  reason 
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Figure  1:  One  developed  dance  partner  robot  [6] 

is  that  waltz  is  the  pHHI  in  well- structured  environment,  the  simple  envi¬ 
ronment  and  simple  context  knowledge  make  waltz  a  good  start  point  for 
understanding  pHHI  and  pHRI.  Therefore,  the  pHHI  and  pHRI  of  waltz 
have  attracted  our  attention. 

Waltz  involves  a  male  dancer  (usually  the  leader)  and  a  female  dancer 
(usually  the  follower).  Understanding  the  female  dancer’s  ability  in  pHHI 
may  help  improving  a  robot’s  performance  in  pHRI;  Therefore,  our  research 
is  to  develop  a  robot  which  can  cooperatively  dance  with  a  human  leader 
with  playing  the  female  dancer’s  role.  A  prototype  developed  in  earlier  stage 
is  shown  in  Fig.  1  [6]. 

The  pHHI  and  the  pHRI  can  be  divided  into  two  levels: 

Intention  estimation  Waltz  has  a  fixed  set  of  motion  patterns,  i.e.,  dance 
steps.  During  dancing,  when  the  leader  selects  the  next  step,  the  fol¬ 
lower  must  estimate  the  leader’s  intention. 

Coupled  dynamics  The  follower’s  body  dynamics  are  coupled  with  the 
leader’s,  hence  the  follower  must  adapt  herself  to  the  coupled  body 
dynamics. 

To  reproduce  the  pHHI  of  waltz  in  pHRI,  the  dance  partner  robot  should 
also  be  able  to  carry  on  the  two  tasks. 

The  higher  level  interaction,  i.e.,  intention  recognition,  has  been  inten¬ 
sively  investigated  in  our  earlier  work  [6,  7]  and  other  researchers’  stud¬ 
ies  [8-11].  In  contrast,  the  lower  level  interaction,  i.e.,  coupled  dynamics, 
is  still  not  well-understood.  In  the  field  of  robotics,  some  dance  robots  that 
can  interact  with  human  have  also  been  demonstrated  by  Khatib  et  al.  [12], 
Oudeyerand  et  al.  [13],  and  Setiawan  et  al.  [14],  etc.  These  works  have  real¬ 
ized  pHRI  from  the  engineering  perspective.  At  the  same  time,  model  and 
analysis  of  the  coupled  dynamics  are  still  not  investigated. 
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Figure  2:  Dancers’  coupled  body  dynamics  in  sagittal  plane  are  studied 


Aside  from  the  pHRI  in  dance,  numerous  methods  have  been  proposed 
in  other  pHRI-related  applications.  Colgate  et  al.  analyzes  the  coupled  sta¬ 
bility  between  a  linear  manipulator  and  a  linear,  passive  environment,  but 
the  explicit  modeling  of  human  dynamics  is  not  implemented  [15].  Kazerooni 
models  the  pHRI  between  a  human  arm  and  a  robot  extender,  and  uses  small 
gain  theorem  to  design  the  robot  controller  which  has  the  guaranteed  sta¬ 
bility,  but  only  the  human  arm  is  modeled,  with  linear,  low-pass-filter  type 
transfer  functions  [16].  The  coupled  dynamics  in  pHRI  has  been  intensively 
investigated  in  the  field  of  haptics  (between  a  human  operator  and  a  hap¬ 
tic  display),  teleoperation  (between  a  human  operator  and  a  robot  master), 
and  human  assistance  [17-23],  in  which  coupled  dynamics  in  pHRI  has  been 
thoroughly,  quantitatively  studied  and  used  for  controller  designs.  However, 
because  of  their  specific  applications,  only  human  arm  dynamics  are  mod¬ 
eled  with  some  passive,  impedance-type  models,  while  the  arm  dynamics  are 
very  different  from  a  dancer’s  body  dynamics  in  waltz.  In  contrast,  human’s 
body  dynamics  in  waltz  are  close  to  a  walking  biped,  whose  dynamics  are 
not  strictly  stable  or  passive. 

Therefore,  to  understand  the  dancers’  coupled  dynamics  in  waltz  and 
utilize  the  knowledge  obtained  to  enhance  pHRI,  new  models,  new  analysis 
and  design  approaches  are  needed. 

On  modeling  the  coupled  dynamics  in  waltz,  we  focus  on  two  dancers’ 
body  dynamics  in  sagittal  plane  (Fig.  2).  Hence  the  limitation  of  the  sim¬ 
plification  is  that  dancers’  rotational  motions  are  not  considered;  however, 
this  one- dimensional  simplified  case  enable  us  to  better  understand  the  fun¬ 
damentals  in  pHHI  and  pHRI.  Similar  one-dimensional  simplifications  also 
appeared  in  literature  in  coordinated  teleoperation  [24] ,  haptic  human-robot 
interaction  [25],  and  human-robot-human  cooperation  [4],  etc. 

Because  our  goal  is  developing  a  dance  partner  robot,  we  aim  to  address 
three  issues: 
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(a)  Double-support  phase 


(b)  Single-support  phase 


Figure  3:  Illustration  of  terms  related  with  walking 


Modeling:  The  coupled  dynamics  in  waltz  are  to  be  modeled; 

Analysis:  System  characteristics  of  the  model  are  to  be  analyzed; 

Control:  Based  on  the  characteristics  of  the  coupled  dynamics,  robot  con¬ 
trollers  are  to  be  designed. 


2  Approach  and  Results 

2.1  Modeling 

2.1.1  A  single  dancer’s  model 

Human’s  body  dynamics  in  waltz  can  be  described  by  a  bipedal  walking 

model;  therefore  here  we  briefly  list  some  terms  related  with  walking: 

Single-support  phase:  The  period  of  time  when  only  one  foot  is  in  contact 
with  ground,  as  shown  in  Fig.  3(b). 

Double-support  phase:  The  period  of  time  when  both  feet  are  in  contact 
with  ground,  as  shown  in  Fig.  3(a). 

Support  leg:  During  single-support  phase,  the  support  leg  is  in  contact 
with  ground  while  supporting  the  body  weight,  as  shown  in  Fig.  3(b). 

Swing  leg:  During  single-support  phase,  the  swing  leg  is  traveling  in  the 
air;  the  swing  leg  has  no  contact  with  the  ground,  as  shown  in  Fig. 
3(b). 

Center  of  mass  (CoM):  The  weighted  average  position  of  all  the  mass  in 
human  body,  as  shown  in  Fig.  3. 
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(a)  Simplified  human  walking  model  (b)  Linear  inverted  pendulum 

Figure  4:  LIPM  as  a  simplified  model 

Center  of  pressure  (CoP):  The  weighted  average  position  of  all  the  pres¬ 
sure  on  the  contact  surface  between  human’s  support  foot  and  ground, 
as  shown  in  Fig.  3(b). 

A  simplified  model  of  walking  is  linear  inverted  pendulum  (LIPM,  [26]). 
Consider  a  simplified  human  body  model  in  sagittal  plane  (Fig.  4(a)).  The 
origin  of  the  coordinate  frame  is  at  human’s  CoP  (center  of  pressure).  By 
applying  several  constraints  on  the  walking  system  [26],  we  can  convert  the 
system  into  an  inverted  pendulum  as  shown  in  Fig.  4(b).  This  system  has 
linear  dynamics  defined  by  [26] 

x  —  -x  4 - /  (1) 

z  m 

where  x  is  the  position  of  LIPM’s  CoM  (center  of  mass)  with  respect  to 
LIPM’s  CoP,  2;  is  the  height  of  CoM,  g  is  gravity  acceleration,  m  is  mass  of 
the  body,  and  /  is  the  external  force. 

Because  of  the  instability  of  LIPM,  a  balance  controller  is  needed.  The 
balance  controller  intermittently  resets  x,  i.e.,  this  inverted  pendulum  can 
instantaneously  reset  its  CoP  to  keep  balance.  Let  {tk}  be  the  set  of  moments 
at  which  the  CoP  resets;  let  x~ ,  x+  be  the  CoM  velocity  before  and  after 
tk]  let  x~ ,  x+  be  the  CoM’s  relative  position  (with  respect  to  CoP)  before 
and  after  tk .  The  LIPM  with  its  controller  can  be  viewed  as  an  impulsive 
dynamical  system  [27]: 

f  x  =  (g/z)x,  t  &  {tk} 

h):>al  ('^  i  ) ,  t  G 

The  function  hbai  in  (2)  is  the  balance  controller.  There  are  many  candidates 
for  the  balance  controller  hbai-  Here  a  proposed  controller  [28]  is  implemented, 
with  x  is  reset  by 

X+  =  _t_r£r;i.^T^  +  ^_Vd^  _|_  1)  (3) 
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Figure  5:  Dancers’  model  as  two  inverted  pendulums 


where  va(k+ 1)  =  x(kTp+Tp)  is  a  reference  input  which  represents  the  desired 
velocity  at  t/~+ 1;  tT  =  y/zjg,  CT  =  cosh (Tp/tr),  CT  =  cosh (Tp/tr),  and  Tp  is 
the  period  of  the  CoP  reset. 

To  test  whether  the  LIPM  can  be  used  in  modeling  a  waltz  dancer,  we 
compare  a  professional  female  dancer’s  motions  with  the  model-generated 
trajectories.  Results  of  comparison  support  the  use  of  LIPM,  while  details 
of  the  experiments  can  be  found  in  [29] . 

2.1.2  Model  of  the  coupled  dynamics 

The  physical  interaction  of  waltz  involves  two  dancers.  If  each  dancer  is  mod¬ 
eled  by  an  LIPM,  then  the  two  physically  coupled  dancers  can  be  modeled 
by  two  spring-damper-connected  LIPMs,  as  shown  in  Fig.  5.  Without  loss  of 
generality,  we  can  consider  the  left  LIPM  as  the  leader,  and  the  right  LIPM 
as  the  follower.  Let  xf  and  x9  be  the  leader’s  and  the  follower’s  CoM  posi¬ 
tions  .  Let  jrj  and  p9  be  their  CoP  positions,  all  with  respect  to  the  global 
frame.  Defining  a  state  vector  x: 

x  =  [xi,Xi,Xf,xf,q]T  (4) 

where  xi  =  x9  —  pj ,  Xf  =  x9  —  p9  are  the  leader’s  and  the  follower’s  relative 
positions  of  CoM  with  respect  to  their  CoPs.  q  =  x9  —  x9  —  dspr ing,  dspring 
is  the  spring’s  natural  length.  kc  and  dc  are  constants  of  the  spring  and  the 
damper.  Dynamics  of  the  two-LIPM  system  are: 

(  x  =  Ax ,  t  {t[}  U  {t{} 

l  x+  =  Htx~  +  Bivld,  t  e  {/;,.}  (5) 

l  x+  =  HfX  +  BfVd,  t  G 

where  {t[}  and  {t£ }  are  the  leader’s  and  the  follower’s  respective  CoP  reset 
moments.  vld  and  vd  are  the  leader’s  and  the  follower’s  desired  velocities  at 
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{tlk}  and  {t{},  respectively.  Matrix  A  is  defined  as 


/  0 

1 

0 

0 

0 

\ 

9_ 

_ dc 

0 

dc 

fee 

zi 

mi 

mi 

mi 

0 

0 

0 
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_  dc 

_  fee 

171  f 

zf 

171  f 

V  0 

-l 

0 

l 

0 

/ 

(6) 


where  zi,  Zf  are  the  leader’s  and  the  follower’s  CoM  heights  and  rrp,  rnj  are 
their  body  mass.  Hi,  Hf,  Bi,  and  Bf  are  the  matrix  forms  of  (3),  with 


Hi=  ( 

(  0 

0 

-QtJSt 

l 

02x3 

\ 

03x2 

-^3x3 

(7) 


/  I 2x2 

02x3 

0 

—CftTf/Sf 

0 

03x2 

0 

1 

0 

0 

0 

1 

/ 

(8) 


and  Bi  =  [tn/Su  0, 0, 0, 0]T,  Bf  =  [0, 0,  tTJSf,  0, 0]T.  HtHf  =  HfHt  =  H. 
Symbols  like  tTlf,  C'ij  and  Sij  are  defined  similarly  as  in  (3).  Interaction 
force  between  the  two  LIPMs  is  denoted  by  /,  with  /  =  ccx,  where 


cc  [0,  dc ,  0,  dc ,  kc 


(9) 


2.2  Analysis 

2.2.1  System  model  with  synchronization  errors 

Since  the  coupled  dynamics  have  been  modeled  by  (5),  we  can  analyze  sys¬ 
tem  stability.  In  our  earlier  work,  by  assuming  two  dancers’  CoP  resets 
are  precisely  synchronized,  stability  of  the  simplified  dynamics  has  been  an¬ 
alyzed  [30].  However,  in  real  applications  two  dancers’  timing  errors  (or 
synchronization  error)  are  inevitable.  Therefore,  it  is  necessary  to  consider 
the  effect  of  timing  error  on  system  stability. 

Let  the  leader’s  and  the  follower’s  k- th  CoP  reset  moment  be  tlk  and  tk 
(Fig.  6).  Because  in  pHRI  the  follower  is  a  robot,  we  can  keep  tk  >  tlk  for  all 
k,  i.e., 

0  <  *1  ~  A  -  H  <  Stf,Vk  (10) 

where  5t[  is  the  follower’s  timing  error  in  following  the  leader,  and  Stf  is  Stk: s 
upper  bound. 


Figure  6:  Timing  errors  of  the  leader  and  the  follower 


Consider  the  timing  error  of  the  human  leader  and  the  robot  following, 
we  have 

-  Sti/2  <  (54+1  =  4+1  -tfk-Tp<  Sti/ 2,  Vfc  (11) 

The  assumptions  of  bounded  timing  errors,  i.e.,  0  <  Stf  <  Stf  and 
—Stj/2  <  St[  <  Sti/2  are  reasonable  because  if  Stf  or  K  grows  unbounded, 
the  two  dancers  would  be  unable  to  dance  together. 

System  dynamics  from  tf+  to  4+ 1  consists  four  transitions,  namely 

J C  I  7 

1.  From  tJk  to  tk+l ,  during  which  there  is  no  CoP  reset  moment;  system 
dynamics  are  continuous; 

2.  From  4+i  to  4+u  the  leader’s  CoP  reset  moment  with  impulsive  dy¬ 
namics; 

3.  From  44 i  to  4+d  continuous  dynamics; 

4.  From  4+ 1  to  4+i>  the  follower’s  CoP  reset  moment  with  impulsive 
dynamics. 

According  to  (5),  (10),  (11),  we  have 

cc(4+i)  =  HfeASt*+iHleA(Tp+Stk+i)x(tfk+) 

+H!eA<^Blvld  +  B,v>  (12) 

Equation  (12)  describes  a  discrete  linear  system.  Its  homogeneous  form 
is: 

*(4+i )  =  A*d(stk+i,  Stl+i)x(tfk+)  (13) 

where 

A i(St‘ul,5t{+1)  =  HfeA<^HieAlT-*s'^  (14) 

and  the  entries  of  A*d(5tlk+1,5tf+l)  depend  on  Stlk+1  and  Stf+1,  which  are  both 
time- varying. 


2.2.2  Stability  of  the  coupled  dynamics 

To  analyze  the  stability  of  the  uncertain,  time- varying  system  in  (13),  we 
introduce  a  stability  condition  proposed  by  Daafouz  et  al.  [31].  To  apply 
this  stability  condition,  firstly  the  time-varying  matrix  Ad(5tlk,  5tk)  is  to  be 
converted  into  a  linear  matrix  polytope  with  the  following  form: 

N 

A‘d(Stl5tt)  =  '£mAi  (15) 

1=1 


N 


&(*;)>  o,  5>(*0  =  i  (16) 

i=l 

Details  of  the  conversion  can  be  found  in  [28] ;  finally  A*d  is  converted  into 
(15),  with 


where 


Ax 

Ai 

A3 

A4 


a'  -  —A'  -  ^-A' 

Ai  2  2  ^4’ 

Uk)  =  1  -  -  Uk)  -  UV 

Ai  +  3 M,A'2,  Uk)  =  ^ 

odtl 

_  x+f 

Ai  +  3 fit/ A3,  £3(k)  =  -== 

3  otf 

sAsti  +  <%/2 


Ai  +  35tifA'4,  £1  (A-)  — 


3  St. 


if 


a;  =  HfHteATp 
A'2  =  HfHieATp  A 
A3  =  HfAHte AT* 
A'4  =  HfAHjeATv  A 


(17) 


(18) 


The  above  system  is  poly-quadratically  stable,  if  and  only  if  there  exist 
four  symmetric  positive  definite  matrices  Si  . . .  S4  >  0,  and  four  regular  ma¬ 
trices  G\  . . .  G4,  which  satisfy  the  following  linear  matrix  inequality  (LMI): 


f  Gi  +  Gj-Si  GjAj  \ 

\  AiG,  Sj  )>u 


(19) 


for  alH  =  1, . . . ,  4  and  j  =  1, . . . ,  4.  In  another  word,  testing  stability  of  the 
two-LIPM  system  is  equivalent  to  examining  the  feasibility  of  (19). 
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Figure  7:  Poly-quadratic  stability  with  respect  to  Stf  (2- axis),  kc  (y- axis), 
and  dc  (a;- axis);  regions  that  have  “temperature”  greater  than  0  correspond 
to  the  stable  combinations  of  Stf,  kc  and  dc 

2.2.3  Numerical  results 

Because  the  stability  test  involves  the  process  of  numerically  solving  the 
LMI  problems,  it  is  extremely  difficult  to  find  a  closed- form  expression  which 
describes  the  relationship  between  stability  and  synchronization  error’s  upper 
bound  (Sti  and  Stf).  Therefore,  we  numerically  analyze  system  stability 
under  different  conditions. 

As  system  stability  involves  4  factors  (Sti,  Stf,  Ay,  and  dc)  which  are 
difficult  to  be  visualized  in  a  3-D  Cartesian  space,  we  assume  Sti  =  0.1  s  (i.e., 
the  human  leader’s  period  error  is  smaller  than  0.1s,  which  is  a  reasonable 
assumption)  and  visualize  the  other  3  factors  in  Fig.  7. 

Three  contours  with  different  Stf  are  shown  in  Fig.  8. 

According  to  Fig.  7  and  Fig.  8,  smaller  Stf  leads  to  larger  stable  region 
of  kc  and  dc ,  which  is  in  line  with  our  intuition  [28]. 


11 


Connection  damping  (Ns/m) 


Connection  damping  (Ns/m) 


(a)  Sti  =  0.1  s ,  Stf  =  0.2  s  (b)  5ti  =  0.1  s,  5tf  =  0.1  s 


(c)  Sti  =  0.1  s,  5tf  =  0  s 


Figure  8:  Contours  of  poly-quadratic  stability  with  respect  to  kc  and  dc: 
dark-colored  area  inside  the  0  border  corresponds  to  poly-quadratically  stable 
combinations  of  kc  and  dc 


2.3  Sensing  Human 

Besides  interaction  force  in  pHRI,  the  robot  also  needs  extra  information  to 
cooperate  with  the  human  dancer.  The  information  can  be  divided  into  two 
categories: 

1.  Timing  of  dance:  The  robot  needs  to  know  the  timing  of  dance  to  reset 
its  CoP; 

2.  State  of  human  dancer:  The  robot  needs  to  know  the  human  dancer’s 
state,  e.g.,  CoM  velocity  and  position. 

Additional  sensors  are  needed  to  measure  those  information.  In  the  subse¬ 
quent  part  we  will  introduce  how  to  sense  the  timing  of  dance 

2.3.1  Sensing  timing  of  dance 

Two  laser  range  finders  (LRF,  Hokuyo  UBG-04LX-F01,  [32])  are  installed  on 
the  robot,  as  shown  in  Fig.  9.  One  LRF  is  installed  (at  human’s  waist  height) 
for  measuring  human’s  waist  position,  while  the  other  LRF  is  installed  (at 
human’s  ankle  height)  for  measuring  ankles’  positions. 

With  the  two  LRFs,  human’s  waist  and  ankles  can  be  identified;  their 
centroids  are  computed  to  represent  their  positions,  as  shown  in  Fig.  10. 

When  human  is  walking,  the  CoP  reset  corresponds  to  the  landing  of  the 
swing  foot,  while  the  landing  of  the  swing  foot  can  be  detected  by  analyz¬ 
ing  the  spatial  information  of  human’s  ankles.  Actually,  by  observing  the 
trajectories  of  the  two  ankles  with  LRFs,  the  robot  can  sense  much  human 
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Figure  9:  The  robot  has  implemented  one  force/torque  sensor  in  waist,  and 
two  LRFs  at  ankle  and  waist  height 
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Figure  10:  A  combined  range  image.  Image  of  two  ankles  are  obtained  from 
the  LRF  on  the  ankle  height,  while  the  upper  body  image  is  from  another 
LRF  installed  at  the  waist  height.  The  markers  are  the  respectively  computed 
centroids  of  the  waist  and  the  ankles 
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Figure  11:  Detecting  foot  landing/s  winging  moments  from  ankle  velocities. 
Black  curves:  ankle  velocities.  Red  impulses:  detected  landing/swinging 
moments,  moments  between  the  paired  impulses  are  double-support  phases 

dancer’s  information.  Besides  CoP  reset  moments,  we  are  also  interested  in 
inferring  human’s  single-support  phase  and  double-support  phase. 

The  beginning  of  double-support  phase  is  represented  by  the  landing  mo¬ 
ment  of  the  swing  foot;  similarly,  the  ending  of  this  phase  is  related  with  the 
initiated  motion  of  the  (previous)  support  foot.  Therefore,  the  two  ankles’ 
velocity  curves  are  used  to  estimate  the  beginning  and  ending  moments  of 
double-support  phase.  Using  the  two  ankles’  velocity  information,  we  can  in¬ 
fer  human’s  timing  (i.e.,  the  beginning  and  ending  of  double-support  phase) 
with  a  simple  threshold-based  method.  The  idea  of  this  method  is  to  find 
the  “falling  edge”  and  “rising  edge”  on  the  velocity  curves  of  the  swing  foot 
and  the  support  foot,  respectively.  The  detected  moments  are  shown  in  Fig. 
11. 

2.3.2  Sensing  state  of  human 

With  the  two  LRFs,  we  can  get  positions  and  velocities  of  the  centroids  of 
human’s  waist  and  ankles.  Let  pwst  and  /)wst  be  the  position  and  velocity  of 
the  waist  centroid  (in  sagittal  plane);  let  pspt  be  the  centroid  position  of  the 
support  foot.  To  get  human’s  state  as  an  LIPM  (including  x  and  x ),  one 
straightforward  method  is  to  use 

[*U  %]  [Pwst  Pspt  dpflp,  pwst]  (20) 

where  o?lrf  is  the  two  LRFs’  installation  offset,  which  can  easily  be  measured 
or  calibrated. 

However,  the  method  given  in  (20)  is  not  practical  due  to  the  following 
problems: 
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1.  pWst  is  not  the  real  CoM  position.  The  real  CoM  position  cannot  be 
directly  measured  by  LRF,  instead,  LRF  can  only  measure  the  surface 
points  on  human  body;  there  is  a  bias  between  their  centroid  and  the 
real  CoM  position;  this  bias  could  be  time-varying.  We  denote  this 
bias  with  dST f  +  5sr f,  where  dsrf  is  its  static  component,  and  5srf  is  the 
time- varying  compoment; 

2.  pspt  is  not  the  real  CoP  position.  During  walking,  human  CoP  is  “trav¬ 
eling”  in  the  contact  surface  between  the  foot  and  the  floor.  This  error 
is  denoted  by  hcoP 


3.  pwst,  Pwst,  and  pspt  contain  large  noises. 

The  listed  problems  can  be  formulated  in  the  following  way.  Firstly,  we 
define  the  sensors’  observation  as 


„  _  f  Pwst  —  Pspt  —  hLRF  —  dSI { 

y“*  "  !  Pwst 

Assuming  the  sensors’  measurement  noise  is  [ui,f2]T,  then 

X  T  hsrf  T  A,'oP 


2/out 


Pwst  Pspt  ^LRF  ^srf 
Pwst 


X 


Vl 

V2 


(21) 


(22) 


LIPM’s  state  is  x  =  [  x,x]T.  Define  S0  =  hsrf  +  <icoP,  the  LIPM’s  dynamics 
in  single-support  phase  are 


x  = 


0  1 
g/z  0 

2/out  =  [x  +  So,  x]T  +  [v1,v2]t 


x  +  [0,  l/m]T/  +  [w1,w2]rj 


(23) 


where  up ,  w2  are  process  noises,  representing  the  unmodeled  dynamics  of 
human  dancer. 

According  to  (23),  inferring  human’s  state  is  equivalent  to  observing  x 
from  j/out j  which  contains  unknown,  time- varying  offset  SQ  and  measurement 
noise  i’i,  v2.  x  can  be  observed  by  combining  the  model  knowledge  and  the 
sensor  measurements;  this  is  realized  by  implementing  a  Kalman  filter.  At  the 
same  time,  the  Kalman-filter-based  estimation  are  only  valid  when  human  is 
in  single-support  phase;  when  the  human  leader  is  in  double-support  phase, 
the  Kalman  filter  is  paused. 

To  evaluate  whether  the  Kalman  filter  can  be  used  for  estimating  the 
human  leader’s  state,  two  experiments  are  directed.  In  both  experiments, 
human’s  body  configurations  are  measured  and  recorded  by  a  motion  capture 
system  (8  Raptor-E  cameras  from  Motion  Analysis  Corp.)  at  the  rate  of 
100  Hz.  The  difference  between  the  two  experiments  are: 


15 


Human 


Figure  12:  Motion  capture  system  is  used  to  measure  human’s  body  config¬ 
uration;  a  force  plate  is  used  to  measure  the  accurate  CoP  position  of  the 
human 

1.  In  the  first  experiment,  a  force  plate  (Kistler  9286A)  is  used  to  measure 
the  traveling  of  human’s  CoP;  due  to  the  limited  size  of  the  force  plate, 
the  human  stays  on  the  force  plate,  with  the  left  foot  supporting  the 
body  and  the  right  foot  swinging  back  and  forth  for  a  couple  of  times 
(Fig.  12).  This  experiment  tests  the  performance  of  the  Kalman  filter 
on  observing  x  and  S0. 

2.  In  the  second  experiment,  the  robot  moves  passively  in  admittance 
control  mode  (which  will  be  discussed  later  in  Section  2.4.1).  Human’s 
motions  are  not  restricted  by  specific  patterns:  he  can  arbitrarily  choose 
his  stride  length  and  walking  speed.  This  experiment  tests  whether  the 
Kalman  filter  can  properly  work  when  being  intermittently  disturbed 
by  human’s  double-support  phase. 

Results  of  the  first  experiment  are  shown  in  Fig.  13.  We  can  see  that  the 
CoM  velocity  x  has  been  accurately  estimated,  while  the  variation  of  offset 
S0  has  also  been  estimated  with  approximately  0.2  s  phase  delay. 

Result  of  the  second  experiment  is  shown  in  Fig.  14.  This  result  shows 
that,  although  the  Kalman  filter  is  intermittently  disturbed  by  human’s 
double-support  phase,  the  proposed  method  can  still  keep  estimating  hu¬ 
man’s  state  in  single-support  phase. 

According  to  the  validation  results,  the  proposed  filter-based  method  can 
be  used  in  state  estimation  of  the  human  dancer. 

2.4  Control 

A  schematic  diagram  of  the  coupled  dynamics  in  waltz  is  illustrated  in  Fig. 
15.  The  system  involves  three  components:  the  human  body,  the  human 
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(a)  Real  and  estimated  CoM  velocities  x  (b)  Real  and  estimated  offsets  5a 

Figure  13:  Experiment  results  on  testing  the  filter’s  performance  on  observing 
x  and  SQ:  red  dashed  curves:  accurate  human  data  measured  by  motion 
capture  system  and  force  plate;  blue  solid  curves:  estimated  human  data 
given  by  the  Kalman  filter 


Figure  14:  Results  of  evaluating  the  proposed  Kalman  filter.  Curves  in  the 
bottom  are  the  measured  (plain  curve)  and  estimated  (curve  with  “+”  mark¬ 
ers)  CoM  positions,  curves  in  the  top  are  CoM  velocities  (plain:  measured; 
“+”  marker:  estimated).  Vertical  blue  lines  are  human’s  timings;  solid:  be¬ 
ginning  of  double-support  phase;  dashed:  ending  of  double-support  phase 
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Intention  of 


Figure  15:  Block  diagram  of  the  human-robot  system  in  waltz,  xi  and  Xf 
are  human’s  and  robot’s  spatial  states  (e.g.,  position,  velocity,  etc).  /  is  the 
interaction  force. 


mf 
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Figure  16:  Illustration  of  the  admittance  controller 


arm,  and  the  robot  follower.  Among  the  three  components,  robot  is  the  only 
component  that  can  be  “manipulated”  by  a  designer.  Therefore,  to  enhance 
the  pHRI  of  waltz,  a  robot  controller  is  to  be  implemented.  Depending  on 
how  the  inputs  are  used  to  generate  the  output,  there  are  infinite  candidate 
controllers,  while  in  this  section  several  of  them  are  introduced. 


2.4.1  Admittance  controller 


Admittance  controller  is  a  widely  adopted  in  pHRI  [34] .  This  controller  only 
uses  interaction  force  as  the  input;  the  idea  of  the  implemented  admittance 
controller  is  that  the  robot  is  emulating  a  mass  driven  by  interaction  force 
and  dragged  by  virtual  ground  friction,  as  illustrated  in  Fig.  16. 

Suppose  the  emulated  mass  is  m/  and  the  emulated  viscosity  coefficient 
is  df]  dynamics  of  the  admittance  controller  are: 


d  f  '  q 

- —x9f  + 

mf  1 


mf 


(24) 


where  x9 ,  and  ir).  are  the  robot’s  position,  velocity,  and  acceleration  with 
respect  to  the  global  coordinate  frame. 

When  the  admittance  controller  is  implemented,  human’s  estimated  state 
is  not  utilized  (i.e.,  consider  the  blue  dash  in  Fig.  15  does  not  exist),  the  inter¬ 
connected  human  arm  and  robot  dynamics  form  a  closed  “loop” ,  as  shown  in 
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Figure  17:  Human  arm  and  robot  form  a  closed- loop  system 


m. 


- ►  / 

f 


Figure  18:  Illustration  of  the  admittance  controller  with  a  virtual  coupling 


Fig.  17.  Because  the  human  arm  model  has  large  uncertainty,  conservative 
stability  conditions  (e.g.,  small  gain  theorem)  are  usually  introduced  in  de¬ 
signing  the  robot  controller  [16].  Because  of  the  conservativeness  in  stability, 
performance  is  sacrificed. 


2.4.2  Admittance  controller  with  virtual  coupling 

The  compromise  between  stability  and  performance  can  be  improved  by  in¬ 
troducing  human’s  spatial  information  into  the  robot  controller  [35]  as  in  Fig. 
15.  Based  on  the  admittance  controller,  we  can  further  implement  a  virtual 
coupling,  which  is  illustrated  in  Fig.  18. 

The  robot  dynamics  are  defined  by 


d 


x 


f 


9  —  _rJ_^9  X  —  (r9  —  t9\  X  —<t9  —  <r9\  -X  — 

f  —  f  iTj  dy  f IT;  Jj  f )  \ 

J  m  n  J  m  n  L  J  m  n  L  J  m 


mf 


mf 


mf 


(25) 


where  kv  and  dv  are  the  stiffness  and  damping  coefficient  of  the  virtual  cou¬ 
pling;  and  xf  are  estimated  human  CoM  position  and  velocity.  The  trans¬ 
fer  function  from  xf  to  x9^  is 


Xf(s)  _  dvs  +  kv 

X9  ( .s )  rrifS 2  +  ( df  +  dv)s  +  kv 


This  controller  is  in  essence  a  low-pass  filter.  It  is  intuitive  as  it  tries  to 
implement  a  virtual  force  to  decrease  the  real  interaction  force  (/);  however, 
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Figure  19:  Illustration  of  the  inverted  pendulum  controller  with  a  virtual 
coupling 


the  performance  of  this  controller  is  also  intrinsically  restricted  by  its  struc¬ 
ture.  Define  the  robot’s  following  error  as  E(s)  =  1  —  Xj(s)/X9  (s),  i.e., 
the  complement  of  (26).  We  can  observe  that  the  parameter  tuning  of  the 
frequency  response  of  E(s)  is  highly  restricted;  quantitative  analysis  of  those 
restrictions  can  be  found  in  [36]. 

2.4.3  LIPM  with  virtual  coupling 

To  better  understand  the  pHHI  between  two  human  dancers,  we  use  the 
inverted  pendulum  presented  in  Section  2.1.1  as  the  robot  model.  At  the 
same  time,  the  measurement  of  human’s  state  is  continuously  rectified  by  a 
Kalman  filter  and  used  as  the  input  to  the  robot  controller.  Diagram  of  this 
controller  is  illustrated  in  Fig.  19. 

Walking  can  be  temporally  divided  into  single-support  phase  and  double¬ 
support  phase  which  have  different  dynamics,  therefore,  the  robot  controller 
also  has  two  phases: 

1.  When  the  robot  detects  that  human  is  in  single-support  phase,  the 
robot  emulates  an  LIPM  with  virtual  coupling: 

Xf  =  —Xf  +  x f  —  x9f)  +  ^-(xi  —  xA  (27) 

2.  When  human  is  in  double-support  phase,  the  robot  switches  to  admit¬ 
tance  control  mode  (24). 

In  the  following  we  explain  how  the  virtual  coupling  appeared  in  (27)  is 
able  to  achieve  interaction  force  reduction.  Define  the  objective  function  as: 

Fob  j  =  /  f2(t)dt  =  /  xT(t)c^ccx(t)dt 

J4+  Jtl+ 


=  zT(4+)Wa:(i{+)  (28) 
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which  means  we  aim  to  minimize  the  interaction  force  throughout  the  single¬ 
support  phase. 

The  virtual  coupling  applies  a  virtual  force  on  the  robot.  Recall  the  two- 
LIPM  system  in  Section  2.1.2,  its  state  matrix  A  is  defined  in  (6),  with  the 
virtual  coupling,  the  state  matrix  A  is  replaced  by  Av,  with 
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Then  the  reduction  of  _F0bj  is 

SF  =  xT(t{+)  (W  -  W„)  *(«£+)  =  xT(tl+) Avx(t 


f+ ) 

k  ) 


(29) 


(30) 


(31) 


If  is  positive  definite,  then  SF  >  0  for  all  x(t(+);  the  interaction  force 
reduction  is  guaranteed.  One  may  think  that  a  positive  definite  Av  can 
be  formulated  by  carefully  adjusting  parameters  ( kv  and  d„ )  of  the  virtual 
coupling;  unfortunately,  this  ideal  situation  is  difficult  to  meet.  We  illustrate 
this  situation  with  an  numerical  example  in  Fig.  20.  According  to  Fig.  20,  it 
is  quite  difficult  to  find  these  parameters;  generally,  Av  is  indefinite,  hence 
the  performance  of  interaction  force  reduction  also  depends  on  the  initial 
condition  x(tl+). 

Therefore,  the  implementation  of  the  virtual  coupling  cannot  guarantee 
that  F0|;,j  is  always  reduced;  instead,  the  virtual  coupling  is  used  to  reduce 
interaction  force  in  most  cases.  Without  loss  of  generality,  we  assume  both 
dancers  are  moving  along  the  positive  direction  of  x-axis.  Their  velocities 
are  assumed  to  be  evenly  distributed  between  [0, 1.5] m/s;  their  relative  CoM 
positions  are  assumed  to  be  evenly  distributed  between  [—0.4,  0]m;  their  dis¬ 
tance  variation  q  is  assumed  to  be  evenly  distributed  between  [—0.2,  0.2]m. 
Performance  of  the  proposed  controller  is  evaluated  with  Monte  Carlo  method. 
106  random  are  tested. 

According  to  the  Monte  Carlo  test,  the  proposed  controller  has  reduced 
F0bj  in  about  95%  cases  with  best  performance  SF  ~  2x  104N2s,  the  controller 
has  failed  in  about  5%  cases  with  worst  performance  SF  ~  — 188  N2s.  The 
probability  density  function  of  SF  is  shown  in  Fig.  21. 

This  probability  density  function  reveals  that  the  proposed  controller  can 
effectively  reduce  interaction  force  with  95%  probability,  while  may  also 
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Figure  20:  Minimum  eigenvalue  of  Av  with  respect  to  virtual  coupling  pa¬ 
rameters 


Objective  function  reduction:  5  F  (N2  s) 

Figure  21:  Probability  density  function  of  SF ;  positive  part  of  the  horizontal 
axis  represents  successful  interaction  force  reduction. 
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Table  1:  Parameters  of  Controller  Used  in  Experiments 


Parameters  used  in  admittance  controller 

mf  (kg) 

50 

df  (Ns/m) 

40 

Parameters  used  in  admittance  controller  with  virtual  coupling 

mf  (kg) 

50 

df  (Ns/m) 

40 

K  (N/s) 

10 

dv  (Ns/m) 

150 

Parameters  used  in  LIPM  controller  with  virtual  coupling 

ms  (kg) 

45 

zf  (m) 

0.9 

K  (N/s) 

0 

dv  (Ns/m) 

225 

slightly  increase  the  interaction  force  with  5%  probability.  Therefore,  the 
proposed  method  is  worth  implementing. 

2.4.4  Experiments 

In  experiments,  we  evaluate  the  above  three  robot  controllers.  Parameters 
for  controllers  are  listed  in  Table  1,  which  is  obtained  after  a  preliminary 
test. 

On  the  hardware  level,  the  robot  controller  has  two  parallel  control  cycles: 

1.  Actuation  of  four  omni- wheels  are  controlled  by  low-level  PD  controllers 
with  1  ms  cycle  period.  In  this  cycle,  few  computations  are  involved. 

2.  LRF  data  processing  and  Kalman  filter  calculation  are  handled  in  high- 
level  cycle;  period  of  this  cycle  is  28  ms. 

For  the  current  hardware,  the  above  computations  approximately  cost  about 
6.1ms  (shorter  than  the  28ms  high-level  cycle).  The  real-time  operating 
system  (QNX  6.1)  ensures  the  1ms  and  28  ms  deadlines  are  satisfied. 
Experiment  results  are  shown  in  Fig.  22. 

As  discussed  in  Section  2.4.2,  although  being  intuitively  plausible,  the 
“admittance  with  virtual  force”  controller  has  poor  performance  in  force 
reduction  (Fig.  22(c)  and  Fig.  22(d)). 
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(a)  Trajectories,  using  admittance  con-  (b)  Interaction  force,  using  admittance 
trol  control 


(c)  Trajectories,  using  admittance  with  (d)  Interaction  force,  using  admittance 
virtual  coupling  with  virtual  coupling 


0.8 

0.6 

0.4 

0.2 

0 

-0.2 


(e)  Trajectories,  using  inverted  pendulum  (f)  Interaction  force,  using  inverted  pen- 
with  virtual  coupling  dulum  with  virtual  coupling 

Figure  22:  Experiment  results.  In  (a),  (c),  and  (e),  curves  with  “+”  markers 
show  robot’s  trajectories  while  plain  curves  show  the  human’s.  For  each 
robot  controller,  the  motion  is  initiated  with  admittance  control  until  the 
end  of  the  first  double-support  phase.  In  (e)  and  (f),  the  robot  switches  to 
admittance  mode  after  six  steps. 
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According  to  Fig.  22(b)  with  Fig.  22(f),  we  can  see  that  the  inverted 
pendulum  controller  (Section  2.4.3)  can  reduce  interaction  force  by  utilizing 
the  the  leader’s  state  estimation,  which  is  being  continuously  rectified  by  the 
Kalman  filter.  Therefore,  the  control  scheme  is  supported  by  the  results. 


3  Conclusions 

To  develop  a  dance  partner  robot  which  can  dance  waltz  with  human,  the 
physical  interaction  between  the  two  dancers  is  studied.  In  addressing  mod¬ 
eling,  analysis,  and  control  for  enhancing  pHRI,  we  make  the  following  con¬ 
tributions: 

1.  A  model  for  describing  dancers’  coupled  dynamics  in  waltz; 

2.  Implementation  of  poly-quadratic  stability  condition  in  proving  the 
two-LIPM  system’s  stability; 

3.  A  novel  method  which  uses  LRF  to  infer  human’s  timing  in  pHRI,  and 
a  Kalman-filter-based  method  for  estimating  the  state  of  human; 

4.  Analysis  and  validation  of  several  robot  controllers. 

However,  our  study  is  limited  as  it  only  models  human’s  translational  mo¬ 
tions  in  sagittal  plane;  dancers’  rotational  motions  are  not  studied.  As  most 
dance  steps  in  waltz  involve  body  rotations,  further  studies  in  understanding 
and  measuring  human’s  rotation  are  needed. 
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Abstract — Haptic  interaction  between  a  human  leader  and  a  robot  follower  in  waltz  is  studied  in  this  paper.  An  inverted  pendulum 
model  is  used  to  approximate  the  human’s  body  dynamics.  With  the  feedbacks  from  the  force  sensor  and  laser  range  finders,  the  robot 
is  able  to  estimate  the  human  leader’s  state  by  using  an  extended  Kalman  filter  (EKF).  To  reduce  interaction  force,  two  robot 
controllers,  namely,  admittance  with  virtual  force  controller,  and  inverted  pendulum  controller,  are  proposed  and  evaluated  in 
experiments.  The  former  controller  failed  the  experiment;  reasons  for  the  failure  are  explained.  At  the  same  time,  the  use  of  the  latter 
controller  is  validated  by  experiment  results. 

Index  Terms — Physical/haptic  human-robot  interaction,  dance,  inverted  pendulum,  extended  Kalman  filter,  laser  range  finder, 
admittance  control. 
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1  Introduction 

altz  is  not  only  an  entertaining  activity,  but  also  a 
typical  example  of  haptic  human-human  interaction 
(HHI)  which  involves  human's  abilities  in  sensing,  control, 
and  coordination.  In  waltz,  two  dancers  are  acting  as  a 
leader  (usually  the  male  dancer)  and  a  follower  (usually  the 
female  dancer),  while  the  follower  can  interact  with  the 
leader  by  "reading"  the  haptic  signals  passed  through  their 
physical  connection.  Understanding  of  the  follower's  ability 
may  help  designing  robots  that  can  be  intuitively  controlled 
through  human-robot  interaction  (HRI).  Therefore,  the  goal 
of  our  research  is  to  develop  a  robot  follower  that  can  dance 
with  the  human  leader  by  utilizing  the  haptic  information.  A 
developed  prototype  of  the  robot  follower  is  shown  in  Fig.  1. 

Generally,  to  interact  with  human,  a  robot  may  refer  to 
models  at  two  different  levels. 

1.  The  higher  level  model  is  used  to  relate  observed 
signals  with  human's  intentions  [1],  [2],  [3],  which 
could  be  named  as  "intention  estimation."  Waltz  has  a 
"vocabulary"  of  various  kinds  of  dance  steps,  while  a 
waltz  dance  consists  of  a  sequence  of  steps.  When  the 
leader  has  selected  the  next  step,  he  is  having  an 
"intention."  The  follower  should  be  able  to  estimate 
this  intention,  as  shown  in  Fig.  2a. 

2.  The  lower  level  model  is  used  for  controlling  the 
dynamics  of  the  interaction  system  in  which  the 
human  and  robot  are  coupled  [4],  [5],  [6],  [7],  [8], 
which  could  be  named  as  "coupled  dynamics."  In 
waltz,  the  follower  is  not  dancing  alone,  her  body 
dynamics  are  coupled  with  the  leader's;  the 
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follower  should  be  able  to  dance  under  the  coupled 
dynamics  without  significant  performance  degrada¬ 
tion,  as  shown  in  Fig.  2b. 

In  one  earlier  work  [9],  the  higher  level  interaction  in 
waltz  has  been  investigated.  To  estimate  the  human  leader's 
intentions,  each  candidate  dance  step  is  modeled  as  a 
hidden  Markov  model  (HMM);  haptic  signals  are  consid¬ 
ered  as  observations  generated  by  one  of  the  HMMs.  For 
each  HMM,  the  likelihood  of  generating  the  given  observa¬ 
tions  is  calculated;  the  model  with  the  highest  likelihood  is 
then  selected  as  the  leader's  "most  probable"  intention. 
Here,  we  only  summarize  this  approach  in  Fig.  3;  details  can 
be  found  in  [9]. 

The  focus  of  this  paper  is  the  lower  level  interaction,  i.e., 
"couple  dynamics,"  which  is  studied  independently  of  the 
higher  level  "intention  estimation."  In  another  word,  fixed 
"dance  step"  is  applied  on  the  two  dancers',  hence  the  HMM- 
based  step  estimator  (Fig.  3)  is  not  involved  in  this  paper. 

More  specifically,  we  focus  on  two  dancers'  coupled 
body  dynamics  in  sagittal  plane  (Fig.  4).  Therefore,  our 
analysis  is  limited  as  it  only  accounts  for  human's 
translational  motions  in  sagittal  and  frontal  planes,  leaving 
rotational  motions  unstudied;  however,  in  waltz,  most 
dance  steps  involve  body  rotations;  pure  translational 
motions  only  correspond  to  a  few  steps  (e.g.,  closed 
changes).  Clearly,  these  simple  translational  motions  are 
still  too  elementary  compared  with  a  dance  partner  robot's 
expected  capability;  however,  those  simple  cases  offer  us  a 
good  start  point  to  explore  the  fundamentals  of  interactions. 
Similar  ID  cases  also  appeared  in  studies  in  coordinated 
teleoperation  [10],  haptic  human-robot  interaction  [11],  and 
human-robot-human  cooperation  [12],  etc. 

In  one  previous  work  [13],  we  assume  that  the  human 
model  has  only  one  parameter — stride  length.  The  robot  is 
able  to  learn  the  human  leader's  stride  length,  which  is  later 
used  to  scale  the  planned  dance  trajectories.  Because  human's 
motion  has  variability  and  randomness,  the  robot  is  also 
under  admittance  control  while  following  the  learned 
trajectory. 

Published  by  the  IEEE  CS,  RAS,  &  CES 


WANG  AND  KOSUGE:  CONTROL  OF  A  ROBOT  DANCER  FOR  ENHANCING  HAPTIC  HUMAN-ROBOT  INTERACTION  IN  WALTZ 


265 


-t 

.. 


Fig.  1.  One  developed  robot  follower. 


(a)  Intention  estimation  (b)  Coupled  dynamics 


Fig.  4.  Dancers’  coupled  body  dynamics  in  sagittal  plane  is  studied. 


(a)  Inverted  pendulum  (b)  Model  in  sagittal  plane 
with  an  extendable  leg 


Fig.  2.  Two  levels  of  interactions  in  waltz. 


Fig.  3.  Estimating  leader’s  intention  using  HMMs. 


Compared  with  a  robot  under  admittance  control, 
inverted  pendulum  can  better  represent  a  human  follo¬ 
wer's  body  dynamics  in  walking  and  dancing.  Therefore, 
we  employed  linear  inverted  pendulum  (LIPM)  [14]  as  the 
human  model;  for  two  dynamically  coupled  dancers,  they 
are  modeled  as  a  pair  of  connected  LIPMs  [15].  By 
assuming  the  two  dancers'  feet  landing  motions  are 
synchronized,  we  analyzed  stability  of  the  system,  while 
interaction  force  was  reduced  with  gradient  descent 
method  [16].  Because  the  assumption  on  two  dancers' 
synchronized  motions  are  too  strong,  this  assumption  was 
later  replaced  by  a  weaker  (bounded  timing  error) 
condition  and  stability  was  examined  [17]. 

To  reduce  interaction  force,  human  leader's  intended 
velocity  is  predicted  and  used  to  control  the  follower's 
motion  [17].  However,  because  of  LIPM's  sensitivity  to  initial 
position  and  velocity  errors,  it  is  very  difficult  to  accurately 
estimate  the  leader's  current  and  future  states. 

To  deal  with  the  above  problem  and  reduce  the 
interaction  force  (for  increased  transparency),  in  this  paper, 
we  propose  a  solution,  which  includes  the  following  parts: 

1 .  A  model  of  human's  body  dynamics  in  sagittal  plane; 

2.  A  method  for  estimating  the  human  model's  state; 

3.  Robot  controllers  which  can  be  used  to  reduce 
interaction  force. 

In  Section  2,  the  inverted  pendulum  model  is  introduced. 
In  Section  3,  methods  for  estimating  human  leader's  state  are 


Fig.  5.  Implemented  model  for  single  dancer. 

described.  In  Section  4,  two  robot  controllers  are  presented. 
Simulation  and  experiment  results  are  shown  in  Section  5, 
discussion  and  conclusions  are  given  in  Sections  6  and  7, 
respectively. 

2  Model  of  the  Human  Dancer  s  Body 
Dynamics  in  Sagittal  Plane 

To  model  the  human  dancer's  body  dynamics  in  sagittal 
plane,  an  inverted  pendulum  model  is  proposed,  which  has 
a  center  of  mass  (CoM),  an  extendable  leg  and  an  ankle  joint 
actuator  (Fig.  5).  Fig.  5a  shows  a  3D  inverted  pendulum. 
This  3D  inverted  pendulum  is  driven  by  gravity  (mg),  leg 
support  (ft),  ankle  torque  (tan k),  and  external  force  (/ext).  As 
stated  in  previous  section,  we  focus  on  the  motions  in 
sagittal  plane,  hence  the  3D  pendulum  in  Fig.  5a  is 
simplified  to  a  2D  model  shown  in  Fig.  5b,  where  fx,  fz 
are  components  of  /ext  in  x  and  z  directions;  f{s,  tq  are 
projections  of  ft  and  tan k  in  sagittal  plane. 

In  waltz,  fx  and  fz  correspond  to  the  interaction  forces 
between  two  dancers,  while  fx  (and  fy  if  considering  frontal 
plane  motion)  is  the  dominant  force;  in  addition,  compared 
with  gravity  mg,  fz  is  much  smaller.  Thus,  we  omit  fz  and 
obtain  the  model  dynamics  in  sagittal  plane 


where  /  =  fx,  %  is  the  relative  position  of  CoM  with  respect 
to  the  pivot  point  (or  center  of  pressure  in  the  supporting 
foot),  2  is  the  height  of  CoM,  m  is  the  mass  of  the  inverted 
pendulum,  g  is  gravitational  acceleration. 

Though  to  appears  as  an  input  in  system  (1),  it  is  at  the 
same  time  an  internal  state  of  the  walking  system.  We  may 
consider  tq  as  a  function  of  x,  x,  and  /;  when  x,  x,  and  /  are 
all  zero,  it  is  reasonable  to  assume  that  re(x,x,f)  =  0.  A 
linear  approximation  of  tq  is  given  by 

to(x,  X ,  /)  =  kix  +  k2x  +  ksf.  (2) 

The  first  two  terms  in  the  right-hand  side  of  (2)  are  the 
damping /actuation  functions  of  the  ankle,  making  the 
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Single  support  Double  support  Single  support 
phase  phase  phase 

Fig.  6.  Switching  dynamics  in  walking. 

inverted  pendulum  less  sensitive  to  initial  errors  (though 
still  unstable).  The  last  term  stands  for  human's  reactions  to 
the  external  force,  with  a  positive  fc3  for  cooperating,  and  a 
negative  fc3  for  resisting. 

The  model  described  in  (1)  and  (2)  only  captures 
human's  simplified  body  dynamics  in  single-support  phase, 
while  in  walking  or  dancing  human  is  switching  between 
single-support  phase  and  double-support  phase,  as  is 
illustrated  in  Fig.  6.  In  double-support  phase,  human  is 
almost  a  fully  actuated  system  (except  that  z  cannot  be 
smaller  than  —g);  the  CoM  position  and  velocity  in  sagittal 
plane  are  fully  controlled. 

In  single-support  phase,  though  human  can  partly 
change  his  trajectory  by  controlling  tq,  this  trajectory  is 
largely  determined  by  gravity  and  external  force  (which  can 
be  measured  by  a  force  sensor).  Further,  tq  itself  may  also 
have  some  repeatable  patterns  in  walking.  The  above  facts 
enable  us  to  model  human  dancer's  dynamics  in  single¬ 
support  phase. 

In  double-support  phase,  the  CoM  trajectory  is  fully 
determined  by  the  human's  joint  torques,  whose  patterns 
are  hard  to  measure  or  model;  therefore,  it  is  quite  difficult 
to  model  this  stage. 

For  a  robot  which  is  haptically  interacting  with  human,  it 
is  necessary  to  deal  with  human's  distinctive  dynamics  in  the 
two  phases.  The  simplest  approach  is  using  a  conservative 
and  passive  robot  model  that  can  work  safely  in  either  phase. 
However,  as  we  have  some  information  about  the  dynamics 
in  single-support  phase,  the  robot  should  utilize  the 
information  to  enhance  interaction  performance  (e.g.,  trans¬ 
parency),  while  staying  conservative  in  double-support 
phase.  In  this  case,  the  robot  should  be  able  to  distinguish 
the  two  phases.  This  is  discussed  in  the  subsequent  part. 

3  Estimation  of  Human  Leader’s  State 

3.1  Detecting  the  Human  Leader’s  Single-Support 
Phase  and  Double-Support  Phase 

In  waltz,  the  switching  between  single  and  double  support 
phases  is  generally  synchronized  with  music  beats.  Inspired 
by  this  fact,  a  straightforward  solution  is  to  extract  music 
beats  from  audio  signals  to  infer  human  leader's  moments  of 
entering/ leaving  double-support  phase.  Also,  numerous 
studies  have  been  directed  on  beat  tracking  from  MIDI  and 
audio  signals  [18],  [19],  [20],  [21],  as  well  as  their  related 
robotic  applications  [22].  However,  reliable  beat  tracking 
from  real  world  sounds  in  real  time  is  still  a  great  challenge, 
while  for  a  robot  which  is  designed  to  be  in  physical  contact 
with  human,  reliability  is  a  crucial  requirement.  Besides 
reliability,  the  audio-based  method  cannot  handle  situations 
in  which  a  skilled  leader  may  intentionally  and  slightly 
dances  "off  beat." 


Fig.  7.  Robot  being  used.  The  robot  has  implemented  one  force/torque 
sensor  in  waist,  and  two  LRFs  at  ankle  and  waist  height. 

Aside  from  audio  signal,  another  cue  that  can  be  used  to 
infer  human's  phase  is  the  spatial  information  of  human's 
feet/ ankles.  For  example,  a  rapid  stop  of  the  left  ankle  means 
the  left  foot  has  landed  on  the  ground,  indicating  that  human 
has  entered  double-support  phase;  similarly,  the  accelera¬ 
tion  of  the  right  ankle  from  zero  velocity  is  related  with  the 
beginning  of  single-support  phase. 

To  measure  human  leader's  ankle  positions  and  velo¬ 
cities,  one  laser  range  finder  (LRF,  Hokuyo  UBG-04LX-F01) 
is  installed  at  human's  ankle  height  (Fig.  7).  A  range  image 
example,  which  shows  what  the  robot  can  "see,"  is  shown 
in  Fig.  8.  Compared  with  the  usually  complicated  range 
images  in  simultaneous  localization  and  mapping  (SLAM) 
research,  the  environment  in  Fig.  8  is  rather  simple  (as  in 
waltz  the  human  leader  is  close  to  the  robot)  and  the  point 
sets  of  two  ankles  can  easily  be  identified.  In  the  following, 
we  will  briefly  explain  the  algorithm. 

1.  Preprocessing.  For  each  frame  (i.e.,  each  range  image 
we  obtained  as  shown  in  Fig.  8),  remove  the 
spurious  points  by  searching  for  line  segments 
shorter  than  a  threshold  (e.g.,  30  mm).  Line  segments 
are  extracted  by  detecting  breakpoints  [23]. 

2.  Identification.  For  each  frame,  identify  human  leader's 
two  ankle  centroids  by  searching  for  the  two  nearest 
clusters  from  the  sagittal  plane;  calculate  leader's 
waist  centroid. 

3.  Measuring  velocity.  Centroid  positions  in  different 
frames  are  used  to  infer  human's  ankle  and  upper 
body  velocities. 

Velocities  of  the  two  ankles  (Fig.  9)  are  used  to  detect  the 
beginning  and  ending  moments  of  double-support  phase. 
Details  of  the  threshold-based  detection  method  can  be  find 
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Fig.  9.  Detecting  human’s  moments  of  entering  and  leaving  double¬ 
support  phase.  The  impulses  are  detected  moments. 

in  [17].  Due  to  the  existed  noise  introduced  by  the  sensor  and 
the  algorithm,  it  is  difficult  to  find  the  exact  moments.  As 
stated  at  the  end  of  Section  2,  because  human's  patterns  in 
double-support  phase  are  hard  to  measure  or  model,  the 
robot  is  supposed  to  act  conservatively  in  double-support 
phase;  to  ensure  a  safe  HRI,  the  ambiguous  moments  should 
be  put  into  double-support  phase.  By  setting  a  relatively 
large  threshold  on  ankles'  velocities,  the  detected  moments 
correspond  to  an  "extended"  double-support  phase,  which 
is  longer  than  the  leader's  real  double-support  phase,  as 
shown  in  Fig.  9. 

3.2  Estimation  of  Leader’s  Intended  Trajectory  in 
Single-Support  Phase 

As  has  been  suggested  in  Section  2,  when  the  human  leader 
is  in  single-support  phase,  his  CoM  trajectory  turns  to  be 
more  patterned;  this  characteristic  can  be  exploited  to 
improve  the  performance  of  HRI.  However,  there  are  also 
several  problems  that  complicate  the  trajectory  tracking  task. 

1 .  The  real  CoM  position  cannot  be  directly  measured 
by  LRF,  instead,  what  the  LRF  can  measure  are  some 
surface  points  on  human  body;  there  is  a  bias 
between  their  mean  position  and  the  real  CoM 
position;  this  bias  could  be  time-varying. 

2.  Equations  (1)  and  (2)  cannot  precisely  model 
human's  body  dynamics  in  single-support  phase. 

3.  The  human  leader's  model  parameters,  e.g.,  m,  z  in 
(1)  and  k\ . . .  k%  in  (2),  are  unknown. 

4.  The  noise  contained  in  the  sensor  feedback. 

The  above  problems  can  be  interpreted  as  follows: 
defining  the  system  state  as  x  —  [x,  x]T,  the  system  described 
in  (1)  and  (2)  are 

X=(°  Mx+  [0,b]Tf+  [w1,w2]T, 

\ai  a2J  (3) 

y=  [x  +  S,x]T  +  [v1,v2]T, 

where  a\  —  (x  +  g)/z  +  ki/{mz),  a2  —  fe/W,  and  b  — 
ks  /  (mz)  +  1  /m  are  human  model's  unknown  parameters, 
and  6  is  the  unknown  bias  of  CoM.  w\  and  w2  are  the 
process  noises,  which  stand  for  the  effects  of  unmodeled 
dynamics;  y  is  a  vector  of  measured  CoM  position  and 
velocity;  v\  and  v2  are  noises  in  the  measurements. 

We  can  include  the  unknown  parameters  in  an  extended 
state  xe: 

xe  =  [x,x,a1,a2,b,8]T,  (4) 


then  the  system  defined  in  (3)  turns  to  be  nonlinear,  with 

%e=  9e(Xe,f)  +  [wi,W2f , 

T  (5) 

y  =  he(xe) +  [v!,v2]  ■ 

By  discretizing  (5),  we  have 

xe{k  +  1)  =  Ge{xe{k),  f(k)) 

+  [w1(k),w2{k)]T,  (6) 

y(k)  =  He(xe(k))  +  [v1(k),v2(k)]T, 

where  Ge  and  He  are  discretized  forms  of  ge  and  he,  with 
the  sampling  period  depending  on  the  LRFs'  scanning  rates. 

The  state  of  the  nonlinear  system  in  (6)  can  be  estimated 
by  an  extended  Kalman  filter  (EKF).  At  the  beginning  of  the 
k  +  1th  time  step,  an  estimated  state  xe(k  +  1)  and  an 
estimated  covariance  matrix  P(k  +  1)  are  predicted 

xe(k  +  1)  =  Ge{xe{k),f{k)), 

P(k  +  1)  =  $(/c)P(fc)$T(fc)  +  <5, 

where  xe{k)  and  P(k)  are  estimation  results  of  the  kth  time 
step,  <!>(&)  is  the  Jacobian  of  Ge,  with  $(/c)  =  (dGe/ 
dxe)\ {xe{k)j{k)yQ  *s  covariance  matrix  of  process  noise  w\ 
and  w2. 

After  P(k  +  1)  is  obtained,  the  Kalman  gain  is  given  by 

K(k+1)- 

P(k  +  l)VT{k)(V(k)P(k  +  l)tfr(fc)  +  Jt)-1, 

where  'P(k)  =  (dHe/dxe)\^~e^j^y  R  is  the  covariance 
matrix  of  measurement  noise  v\  and  v2. 

The  Kalman  gain  in  (8)  is  used  in  building  a  state  observer: 

Xe(k  +  1)  =  xe(k+  1) 

+  K(k  +  1  )(y(k  +  1)  -  He(xe(k  +  1))). 

Thus,  xe{k  +  1)  is  estimated  from  xe(k)  and  P(k).  Finally, 
the  estimation  covariance  matrix  P(k  +  1)  is  updated  by 

P(k  +  1)  =  (/  -  K(k  +  1  )*(k))P(k  +  1).  (10) 

Hence,  xe(k)  and  P(k)  can  be  updated  in  each  time  step.  At 
the  same  time,  the  implementation  of  EKF  requires  the 
nontrivial  task  of  estimating  the  covariance  matrices  Q  and 
R,  because  the  Kalman  gain  is  largely  determined  by  Q  and 
R  (or  more  specifically,  the  "ratio"  between  Q  and  R).  Q 
and  R  can  be  estimated  by  tuning,  or  offline  analysis  of 
recorded  measurements  [24]. 

Generally,  precise  measurement  of  human's  CoM  posi¬ 
tion  in  walking  is  a  great  challenge  [25],  [26].  At  the  same 
time,  as  the  human  dancer's  motion  is  restricted  by  the 
requirements  of  waltz  (e.g.,  the  upper  body  configuration  is 
regulated,  the  feet  should  be  always  in  touch  with  the  floor, 
etc.),  the  traveling  of  CoM  position  is  restricted.  Hence,  large 
error  will  not  be  introduced  if  we  assume  the  CoM  is  in  a 
fixed  position  with  respect  to  the  measured  body  surface. 
According  to  this  assumption,  the  CoM  is  supposed  to  have  a 
constant  bias  from  the  waist  centroid,  while  its  velocity  is 
supposed  to  be  the  same  with  the  centroid's.  The  error 
introduced  by  this  assumption  is  further  reduced  by  the 
continuous  correction  of  the  EKF. 
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Sample  number  (sample  period:  28  ms) 

Fig.  10.  Predicting  leader’s  trajectory  using  EKF.  Solid  curves:  leader’s 
real  CoM  position  (bottom,  thick)  and  velocity  (top,  thin).  Curves  with 
markers:  estimated/predicted  values.  Solid/dashed  vertical  lines:  mo¬ 
ments  of  entering/leaving  double-support  phase. 

Because  the  unknown  parameters  are  also  updated,  the 
EKF  can  be  used  in  predicting  human  leader's  future 
trajectory.  Fig.  10  shows  one  example  of  how  predictions 
are  made.  When  human  leader  leaves  double-support  phase, 
the  EKF  is  enabled;  after  the  state  and  parameter  estimations 
are  updated  with  a  specific  number  of  iterations,  the 
predictor  (which  guesses  future  states  by  using  the  recently 
updated  parameters  and  state  of  the  LIPM)  is  started  to 
predict  the  human  leader's  future  CoM  trajectory.  Due  to 
unmodeled  dynamics  and  initial  estimation  error,  long-term 
prediction  is  less  accurate  than  a  short-term  one  (e.g.,  the 
unexpected  velocity  decrease  in  Fig.  10).  Principally,  rather 
than  using  EKF  as  an  intermittent  predictor  (which  pauses 
updating  parameters  and  tries  to  make  long-term  predic¬ 
tions,  as  shown  in  Fig.  10),  EKF  should  actually  be  used 
continuously,  hence  we  can  keep  updating  the  parameters 
and  correcting  the  errors.  On  the  other  hand,  the  intermittent 
long-term  predictor  can  be  used  to  evaluate  the  quality  of 
EKF  parameters,  e.g.,  an  EKF  with  smaller  long-term 
prediction  error  should  have  less  unmodeled  dynamics  and 
better  estimated  Q  and  R. 

Note  that  the  above  model  and  the  estimation  method 
are  only  valid  when  the  human  leader  is  in  single-support 
phase;  if  human  is  detected  to  be  in  double-support  phase, 
EKF  is  paused. 

4  Controlling  the  Robot 

A  general  representation  of  the  human-robot  system  in 
waltz  is  given  in  Fig.  11.  If  we  do  not  consider  human  body 
dynamics  and  the  feed-forward  path  of  xi  (blue  dash  in 
Fig.  11),  the  interconnected  human  arm  and  robot 
dynamics  form  a  very  typical  and  frequently  encountered 
"loop"  in  the  research  on  haptic  human-robot  interaction. 
For  such  systems,  there  is  usually  a  tradeoff  between 
stability  and  performance.  For  example,  if  the  human  arm 
model  has  large  unstructured  uncertainty,  the  designer  will 
tend  to  use  its  worst  case  gain,  along  with  a  conservative 
stability  criterion  (e.g.,  small  gain  theorem)  to  synthesize 
the  robot  controller  [4]. 

The  compromise  can  be  improved  if  the  estimated 
human's  motion  (xi  in  Fig.  11)  can  be  feed-forwarded  to 
the  robot  controller  [27].  In  the  application  of  waltz,  since 
human  is  underactuated  in  single-support  phase,  his  CoM 
trajectory  can  be  estimated  (as  has  been  discussed  in 
Section  3.2);  the  estimation  can  then  be  sent  to  the  robot 


Intention  of 


Fig.  1 1 .  Block  diagram  of  the  human-robot  system  in  waltz.  x{  and  xf  are 
leader’s  and  follower’s  spatial  states  (e.g.,  position,  velocity,  etc.).  /  is 
the  interaction  force. 

controller  for  achieving  enhanced  performance  (e.g.,  in¬ 
creased  transparency  with  minimized  /). 

The  explicit  inclusion  of  human  body  dynamics  and 
the  feed-forward  path  of  human  states  yield  the  model  in 
Fig.  11.  In  the  following,  we  will  discuss  two  types  of 
robot  controllers. 

4.1  Admittance  Control  with  Virtual  Force 

If  human's  arm  has  internally  stable  dynamics  with  no  other 
unknown  inputs,  then  the  amplitude  of  /  is  largely 
determined  by  xi  —  xf,  in  the  ideal  case,  if  the  robot  can 
perfectly  follow  human,  i.e.,  xi(t)  =  Xf{t),  then  f(t )  — >  0. 
However,  in  practice,  this  simple  method  has  two  problems. 

1.  The  state  cannot  be  estimated  in  human's  double¬ 
support  phase. 

2.  The  estimation  (denoted  by  xi)  contains  (usually 
high  frequency)  noise,  which  cannot  be  directly  used 
to  command  the  actuators  on  the  robot. 

For  the  first  problem,  we  can  have  the  robot  work  in 
admittance  mode  if  the  human  leader  is  in  double-support 
phase. 

To  deal  with  the  second  problem  in  single-support  phase, 
we  can  implement  a  low-pass  filter,  which  can  be  inter¬ 
preted  as  a  virtual  force  (for  convenience  we  discuss  the 
continuous  case) 

fv  =  kv(xi-xf)+dv(xi-xf),  (11) 

where  kv  and  dv  are  designer-specified  constants  of  virtual 
spring  and  damper. 

The  robot  is  working  in  admittance  control  mode,  driven 
by  real  and  virtual  forces 

Wlad'f  T  daXf  —  f  +  fv ,  (12) 

where  ma  and  da  are  virtual  mass  and  damping  of  the 
admittance  controller.  From  (11)  and  (12),  the  transfer 
function  from  xi  to  Xf  is 

^/(^) _  dvs  T  kv  (13^ 

Xi(s)  mas2  +  (da  +  dv)s  +  kv 

which  is  a  low-pass  filter.  Intuitively  this  filter  is  trying  to 
use  a  virtual  force  ( fv )  to  help  decreasing  the  real  interaction 
force  (/),  while  the  robot  is  working  in  admittance  mode. 
Because  the  robot  dynamics  is  stable  and  linear,  this  method 
is  easy  to  understand  and  implement.  In  essence,  (11)  and 
(12)  form  an  admittance  controller  with  the  feed-forward  of 
human's  estimated  trajectory,  this  idea  has  been  proposed 
and  validated  by  Maeda  et  al.  [28]  and  Corteville  et  al.  [29]. 
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1200  1250  1300  1350  1400 

Sample  number  (sample  period:  28  ms) 

(a)  EKF  used  in  estimating  leader’s  state 


(b)  EKF  used  in  predicting  state;  with  18  itera¬ 
tions  in  single-support  phase,  EKF  stops  and 
predictor  is  started;  prediction  is  being  made 
until  the  next  double-support  phase  comes 

Fig.  12.  Results  of  evaluating  the  proposed  EKF.  In  each  figure,  curves 
in  the  bottom  are  measured  (plain  curve)  and  estimated  (curve  with  “+” 
markers)  CoM  position,  curves  in  the  top  are  CoM  velocities  (plain: 
measured;  “+”  marker:  estimated). 


4.2  A  “Human-Like”  Approach 

For  the  case  of  waltz,  there  is  a  major  difference  between 
HHI  and  HRI:  for  HRI,  the  dynamic  model  of  the  robot 
follower  can  be  artificially  created  to  meet  specific  require¬ 
ments.  For  example,  (13)  can  be  replaced  with  other  linear 
or  nonlinear  models  if  the  design  specifications  can  be 
satisfied.  In  contrast,  in  HHI,  the  human  follower  is  subject 
to  her  intrinsic  dynamics,  which  can  hardly  be  changed  by 
an  external  controller. 

Therefore,  to  better  understand  the  haptic  interaction 
between  two  human  dancers,  we  will  use  the  inverted 
pendulum  presented  in  Section  2  as  an  assumed  approxima¬ 
tion  of  the  follower's  body  dynamics. 

In  the  single-support  phase  of  the  robot  follower,  its 
dynamics  are  similar  with  (1)  and  (2),  except  that  the  feed¬ 
forward  of  xi  is  included  in  the  ankle  torque  control 

re  =  hx  +  k2x  +  k3f  +  k±(xi  -  x/),  (14) 

which  models  the  follower's  effort  in  catching  the  leader's 
varying  speed.  In  addition,  to  simplify  the  implementation, 
z  is  set  to  0  and  z  is  set  to  a  constant  value. 

At  the  moment  of  feet  landing,  the  robot  is  assumed  to 
follow  the  leader's  stride,  i.e.,  we  assume  x ^  =  x+.  In  the 
double-support  phase  of  the  robot,  we  have  it  work  in 
admittance  mode. 

It  should  be  noticed  that,  rather  than  being  a  true  model 
of  human  body  dynamics,  the  above  model  is  an  approx¬ 
imation  based  on  the  assumed  "human-like"  behavior. 
However,  compared  with  the  model  in  Section  4.1,  the 
inverted  pendulum  better  catches  human's  unstable  dy¬ 
namics  in  single-support  phase,  hence  this  assumed  "hu¬ 
man-like"  model  is  a  better  candidate  for  reproducing  the 
HHI  in  waltz. 


TABLE  1 

Parameters  of  Simulating  the  Interaction  between 
Two  Inverted  Pendulums 


Parameters 

Leader 

Follower 

m  (kg) 

70 

45 

*  (m) 

[0.9, 1.1] 

0.9 

z  (m/s2) 

[-3,3] 

0 

ki  (N) 

[-100,0] 

-40 

k2  (Ns) 

[-100,0] 

-40 

k3  (m) 

[-50,  50] 

2 

kA  (Ns) 

0 

5 

Connection  stiffness  kc  (N/m) 

[20, 180] 

Connection  viscosity  dc  (Ns/m) 

[0, 0.42fcc] 

5  Simulations  and  Experiments 

Before  HRI  experiments,  our  first  concern  is  that  whether 
our  assumed  model  given  in  Section  2  and  the  extended 
Kalman  filter  discussed  in  Section  3.2  can  be  used  for 
estimating  and  predicting  human  leader's  state.  Therefore, 
we  did  experiments  in  which  the  robot  worked  purely  in 
admittance  mode,  being  "pushed"  by  the  human  leader 
while  trying  to  track  the  leader's  states  with  EKF. 

The  human  leader's  motions  are  not  restricted  by  specific 
patterns:  as  long  as  the  velocity  does  not  exceed  motors' 
speed  limits,  the  leader  can  arbitrarily  choose  his  stride 
length  and  walking  speed.  Five  human  subjects  (age  range: 
22-30;  height  range:  1.60-1.85  m;  weight  range:  50-75  kg)  are 
asked  to  walk  with  the  robot  follower  with  self-selected 
speeds  and  paces.  During  the  above  process,  surface  points 
on  the  human  leader's  waist  and  ankles  are  measured  by  the 
two  LRFs;  leader's  phase  (double /single  support)  are  then 
inferred  and  recorded  along  with  the  measured  interaction 
force.  Then  EKF  is  applied  on  the  recorded  data.  Covariance 
matrices  Q  and  R  are  estimated  by  offline  processing  the 
recorded  measurements  using  ALS  package  [24].  Results 
are  shown  in  Fig.  12. 

According  to  the  results,  the  proposed  model  in  (3)  and 
EKF  in  (7)-(10)  can  be  used  in  state  estimation  and  short¬ 
term  prediction.  At  the  same  time,  the  prediction  error 
increases  as  the  look-ahead-time  grows  larger,  which  makes 
long-term  predictions  less  accurate. 

The  second  concern  is  whether  the  intrinsically  unstable 
inverted  pendulum  can  be  used  to  interact  with  human. 
Although  related  HRI  experiments  have  demonstrated  its 
feasibility  [16],  because  we  have  modified  the  model  and 
included  double-support  phase,  extra  validations  are  neces¬ 
sary.  Before  experiments,  we  modeled  two  dancers  as  two 
inverted  pendulums  (dynamics  described  in  Section  2) 
connected  by  a  spring  and  a  damper,  then  simulated  the 
interaction  between  them.  Parameters  of  the  simulation  are 
listed  in  Table  1. 

The  leader  and  the  connection  are  assumed  to  have 
uncertain,  time-varying,  but  bounded  parameters  (varying 
within  the  given  domain).  Values  of  those  parameters  are 
chosen  due  to  the  following  reasons: 

1 .  The  follower  is  a  robot  emulating  an  virtual  inverted 
pendulum;  parameters  of  the  virtual  pendulum  can 
be  arbitrarily  specified. 
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(a)  Curves  at  bottom:  CoM  positions  of  leader 
(plain  curve)  and  follower  (curve  with  “+” 
marker);  curves  at  top:  CoM  velocities  of 
leader  (plain)  and  follower  (marker) 


(b)  Interaction  force  (c)  Simulated  torque  at 
the  follower’s  ankle 


Fig.  13.  Simulated  interaction  between  two  inverted  pendulums. 


2.  For  the  human  leader,  m  is  the  author's  mass;  z  is  the 
57  percent  of  the  author's  height;  z  is  from  a  dancer's 
motion  capture  data  in  the  pelvis.  Because  k\ . . .  ks 
are  human  dancer's  internal  control  gains  that  are 
hard  to  measure,  they  are  given  large  uncertain 
bounds.  As  human  is  the  leader,  is  set  to  0. 

3.  kc  and  dc  can  be  inferred  from  recorded  interaction 
force  and  two  dancers'  distance.  A  preliminary 
analysis  shows  that  (perhaps  due  to  different  tasks) 
kc  is  smaller  than  the  experiment  results  given  in  [30]. 
The  results  are  used  as  the  upper  bound  of  kc 
(0  <  kc  <  400),  while  dc's  upper  bound  is  0.42/cc 
(0  <dc<  0.42/cc)[31].  With  the  parameter  constraints, 
kc  and  dc  are  then  identified  by  minimizing  the 
experiment  data  fitting  error.  Because  kc  and  dc  could 
be  time-varying,  the  experiment  data  are  partitioned 
and  fitted,  respectively.  kc  and  dc/  in  Table  1,  are 
results  of  the  identification. 

Besides  the  time-varying  and  uncertain  parameters,  the 
two  dancers  also  have  random  but  bounded  timing  errors 
in  entering /leaving  double-support  phase.  Random  noise  is 
also  included  in  the  measurements. 

The  robot  does  not  know  the  leader's  trajectory  in  advance, 
but  tries  to  follow  the  leader  with  the  controller  proposed  in 
Section  4.2.  Simulation  results  are  shown  in  Fig.  13.  We  can 
see  that  in  simulation,  the  "human-like"  controller  can  drive 
the  robot  following  the  leader's  trajectory,  regulating  the 
interaction  force  as  well  as  keeping  the  ankle  torque  within  a 
reasonable  amplitude. 

After  the  above  evaluations,  experiments  are  directed. 
The  control  program  of  the  robot  follower  is  running  on  a 
single-board  computer  with  995  MHz  (benchmark  result) 
Pentium  III  CPU  and  256  MB  memory.  The  operating 
system  is  QNX  6.1. 

The  program  has  two  parallel  control  cycles. 

1.  Four  servo  motors  (which  drive  four  omniwheels) 
are  controlled  by  PD  controllers  running  on  the 


low-level  cycle,  with  1ms  cycle  period.  This  cycle 
contains  few  computations. 

2.  The  high-level  cycle  contains  LRF  data  processing 
and  EKF  calculation,  which  cost  most  of  the  computa¬ 
tion  time.  Period  of  the  high-level  cycle  is  28  ms. 

The  EKF  needs  213  add  operations,  154  subtracts, 
391  multiplies,  and  4  divides.  The  LRF  data  processing  needs 
252  adds,  504  multiplies,  and  504  (worst  case)  trigonometric 
function  calls,  with  all  operations  on  double-precision 
numbers  (64  bits  in  current  platform).  For  the  CPU  being 
used,  the  above  computations  approximately  cost  6.5M  clock 
cycles  in  the  worst  case,  corresponding  to  about  6.5  ms,  which 
is  shorter  than  the  high-level  cycle  (28  ms).  The  operating 
system  (QNX  6.1)  guarantees  the  1ms  and  28  ms  deadlines  are 
satisfied  in  hard  real  time:  if  the  computation  time  once 
exceeds  the  limit,  the  program  will  be  stopped  with  an  error 
report.  The  program  is  then  tested  to  ensure  that  the 
computation  time  can  satisfy  the  hard  real-time  requirement. 

Three  different  robot  controllers:  pure  admittance, 
admittance  with  virtual  force  (Section  4.1)  and  inverted 
pendulum  (Section  4.2),  are  examined.  The  five  human 
subjects  are  asked  to  walk  with  the  robot  follower  with  self- 
selected  stride  lengths  and  restrictions  on  the  paces  (should 
be  faster  than  1  step  per  second). 

Parameters  for  admittance  control  are  ma  =  50  kg, 
da  =  40  Ns/m;  for  generating  virtual  force,  kv  =  10  N/m 
and  dv  —  150  Ns/m.  For  inverted  pendulum,  we  set  m  = 
45  kg,  z  —  0.9  m,  z  =  0,  k\  —  —40  N,  —  —40  Ns,  k%  =  2  m, 
and  &4  =  5  Ns.  The  above  parameters  are  selected  after  a 
preliminary  test  before  experiments. 

Experiment  results  are  given  in  Fig.  14.  Comparing 
Fig.  14b  with  Fig.  14f,  it  can  be  seen  that,  when  using  the 
inverted  pendulum  controller  (Section  4.2),  the  interaction 
force  can  be  reduced  by  utilizing  the  feed-forward  of  the 
leader's  state.  Therefore,  the  control  scheme  which  uses 
feed-forward  (Fig.  11)  and  a  "human-like"  controller  is 
supported  by  the  experiment  results. 

However,  despite  its  intuitive  feasibility,  the  "admittance 
with  virtual  force"  controller  in  Section  4.1  demonstrates  no 
evident  enhancement  in  transparency  (Figs.  14c  and  14d). 
Originally  we  attributed  the  failure  to  the  wrong  selections 
of  controller  parameters;  however,  many  sets  of  parameters 
tested  and  tuned  in  experiments  still  result  in  no  improve¬ 
ment.  In  the  subsequent  part,  we  will  give  a  tentative 
explanation  on  the  failure  of  this  controller. 

6  Discussion 

6.1  About  the  Virtual  Force  Controller 

In  developing  the  virtual  force  controller  in  Section  4.1,  we 
used  the  model  in  (13),  which  will  drive  the  robot  to  follow 
the  human  leader's  estimated  trajectory.  Here,  we  define  the 
robot's  following  error  as  E(s)  =  1  —  Xf(s)/Xi(s),  i.e.,  the 
complement  of  (13),  we  have 

\  _  Xi(s)  —  Xf(s)  _ _ s(mas  +  da) _ 

A/(s)  uias^  T  (da  T  dv^js  T  kv  (15) 
_  s(s  +  u;w) 

(5  +  Ld\)  (s  +  UJ2) 

with  da  —  ma uu,  dv  —  (00 1  +  uj2  -  uu)ma,  and  kv  —  ujiuj2ima. 
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(a)  Trajectories,  using  ad-  (b)  Interaction  force,  using 
mittance  control  admittance  control 


(c)  Trajectories,  using  ad-  (d)  Interaction  force,  us- 
mittance  with  virtual  force  ing  admittance  with  virtual 

force 


20  dB/decade,. 

40  dB/decade 

20  dB/decade 

0  dB/decade 

0)u  COx  C£>2 

(a)  uju  <  (jOi  <  (j2 

20  dB/decade 

0  dB/decade 

r^dB/decade 

0  dB/decade 

a 

\  a 
(b)  wi 

<Uu<CJ' 

>2 

2 

20  dB/decade 

0  dB/decade 

-20  dB/decade 

0  dB/decade 

°\  ®2 

(C) 

Fig.  15.  Possible  Bode  plots  of  |£(s)|. 

dv  =  (v i  +  uj2-  uu)ma  w  100ma,  (16) 


(e)  Trajectories,  using  in-  (f)  Interaction  force,  using 
verted  pendulum  inverted  pendulum 

Fig.  14.  Experiment  results.  In  (a),  (c),  and  (e),  curves  with  “+”  markers 
show  robot’s  trajectories  while  plain  curves  show  the  human’s.  For  each 
robot  controller,  the  motion  is  initiated  with  admittance  control  until  the 
end  of  the  first  double-support  phase.  In  (e)  and  (f),  the  robot  switches  to 
admittance  mode  after  six  steps. 

In  frequency  domain,  the  behavior  of  E(s)  is  fully 
determined  by  ui,  uj2/  and  uju\  on  the  Bode  plot  of  \E(s)\,  at 
frequencies  of  ui  and  uj2,  the  slope  of  the  magnitude  curve 
decreases  by  20  dB  per  decade,  while  at  uju  the  slope 
increases  by  20  dB  per  decade.  Without  loss  of  generality, 
we  assume  u o2  >uo\.  Depending  on  the  values  of  uji,  uj2,  and 
uju,  there  are  three  possible  Bode  plots  of  \E(s)\;  we  list  them 
in  Fig.  15. 

In  all  the  three  possible  cases  in  Fig.  15,  amplitude  of  the 
following  error,  i.e.,  \E(s)\  approaches  0  at  low  frequencies 
and  100  percent  at  high  frequencies  (because  Xf(s)  is  the 
low-pass-filtered  output  of  Xi(s)).  At  the  same  time,  the  case 
in  Fig.  15c  is  nonoptimal  since  \E(s)\  is  amplified  at  a 
specific  frequency  band,  therefore  the  case  in  Fig.  15c  will 
not  be  considered. 

In  the  two  cases  in  Figs.  15a  and  15b,  we  have 
uj2  >  max(cui,o;w).  And  the  slope  before  uj2  is  20  dB  per 
decade.  To  realize  effective  attenuation  of  \E(s)\  (e.g., 
\E(s)\  <  -20  dB),  we  should  have  uj2  to  be  a  decade  larger 
than  max((Ji,  lju),  i.e.,  uj2  >  10  x  max(wi,  uu),  and  place 
max(cui,cjw)  around  human's  walking  frequency  (approxi¬ 
mately  2  Hz,  or  around  10  rad/s);  this  arrangement  of 
max(cui ,  uju)  will  give  us  at  least  —20  dB  attenuation  of  |i£(s)  |. 
Therefore,  we  can  consider  max (uu,  cui)  «  10  rad/s,  and 
uj2  m  100  rad/s,  which  yields 


—  =  cou<  10.  (17) 

ma 

Equations  (16)  and  (17)  suggest  a  dilemma  we  are  facing. 

1 .  If  ma  is  large,  e.g.,  50  kg  as  used  in  experiment,  then 
dv  =  5,000,  which  will  greatly  amplify  the  (already 
large)  noise  contained  in  xd 

2.  If  ma  is  small,  then  according  to  (17),  da  must  also 
decrease  in  proportion;  however,  values  of  ma  and 
da  are  restricted  in  admittance  control:  due  to  the 
noise  introduced  by  force  sensor  and  the  conserva¬ 
tiveness  of  the  small  gain  theorem,  ma  and  da  should 
be  sufficiently  large. 

Generally,  because  a  robot  is  expected  to  be  safe, 
insensitive  to  noise,  and  easy  to  analyze /implement,  we 
usually  use  stable,  linear  and  low-pass-filter  type  robot 
models,  which  can  be  written  as  a  strictly  proper  and  stable 
transfer  function  (e.g.,  (13)).  As  the  complement  of  robot 
model,  the  following  error  is  not  strictly  proper  and  will 
finally  reach  0  dB  as  frequency  increases.  It  is  a  nontrivial 
task  to  tune  the  controller's  parameters  to  optimize  the 
frequency  response  while  satisfying  the  existing  restrictions 
on  stability.  If  the  sensors  have  large  signal-to-noise  ratio,  or 
if  the  human  model  contains  small  uncertainty,  then  it  is 
possible  to  improve  system  performance  with  appropriate 
parameters,  e.g.,  as  the  case  in  [28];  in  contrast,  in  our 
application,  the  large  noise  contained  in  the  inferred  human 
CoM,  as  well  as  in  the  interaction  force,  greatly  limited  our 
parameter  selection  to  a  narrow  range.  The  parameters  used 
in  experiment  put  much  weight  on  stability  and  noise 
attenuation,  hence  performance  was  sacrificed,  e.g.,  at 
frequency  of  10  rad/ s,  the  magnitude  of  |i£(s)  |  is  only  -0.3  dB. 

If  the  robot  model  is  not  constrained  to  be  linear,  stable 
or  proper,  generally  we  will  have  more  design  space  to 
optimize  the  interaction.  The  inverted  pendulum  model 
used  in  Section  4.2  exemplifies  the  use  of  an  intermittently 
unstable  model.  At  the  same  time,  compared  with  the  linear 
and  stable  case,  our  knowledge  in  the  broader  design  space 
is  still  quite  limited;  there  are  a  lot  of  control  methods  to  be 
explored  in  the  haptic  human-robot  interaction. 
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6.2  Dancers’  Body  Dynamics  in  Turning  Maneuvers 

As  stated  in  Section  1,  the  planar  model  in  Fig.  5b  only 
describes  dancer's  translational  motions.  To  fully  account 
for  dancers'  complete  motions  in  waltz,  their  body  dynamics 
in  turnings  (i.e.,  rotations  around  the  vertical  axis)  should  be 
modeled  and  analyzed. 

Compared  with  the  "normal"  turning  maneuvers  in 
walking,  the  turnings  in  waltz  have  several  different  features. 

1 .  The  turnings  usually  have  large  rotation  angles,  e.g., 
natural  turn  (name  of  a  dance  step)  consists  of  two 
successive  turnings,  corresponding  to  two  135  degree 
clockwise  rotations. 

2.  Turnings  mostly  happen  in  single  support  phase.  In 
addition,  for  the  three-beat  waltz  music,  the  rotation 
usually  starts  around  the  second  beat  and  ends 
around  the  third  beat. 

3.  The  dancer  usually  keeps  a  fixed  upper  body 
configuration  during  turning,  with  the  swing  foot 
sliding  on  the  floor. 

Human's  turning  maneuvers  during  walking  have  been 
measured  and  analyzed  [32],  [33],  [34],  suggesting  the 
complicated  body  dynamics  and  control  in  turning  maneu¬ 
vers.  At  the  same  time,  some  simplified  models  have  been 
introduced  to  generate  turning  motions  on  bipedal  robots.  A 
3D  model  and  its  controller  proposed  by  Shih  et  al.  [35] 
guarantee  the  input-to-state  stability  during  turning,  but  the 
model  has  point  feet  and  the  controller  only  allows  small  turn 
angles.  A  friction-based  method  proposed  by  Miura  et  al.  [36] 
can  generate  larger  turn  angles,  but  the  turnings  happen  in 
double-support  phases. 

A  bipedal  turning  model,  which  is  suitable  for  modeling 
dancer's  turning  maneuvers  in  waltz,  has  been  recently 
proposed  by  Kim  and  Park  [37].  Including  upper  body 
moment  inertia,  swing  foot,  ankle  torque,  and  ground 
friction,  this  model  can  describe  dancer's  turnings  of  large 
rotation  angles  in  single-support  phase.  For  a  waltz  dancer, 
if  an  additional  constraint  (zero  height  of  the  swing  foot)  is 
introduced,  this  model  can  be  further  simplified. 

Although  we  may  have  a  suitable  model,  there  are  still 
two  challenges. 

1.  How  turning  is  controlled  by  human  is  unknown; 
e.g.,  the  way  that  the  rotation  is  accelerated/ 
decelerated  by  ankle /hip /friction. 

2.  It  is  difficult  to  measure  human  leader's  angular 
position  with  LRFs. 

The  two  challenges  are  to  be  addressed  to  extend  the 
human-robot  interaction  to  more  complete  motions  in  waltz. 

6.3  Switching  of  Leader-Follower  Role 

On  the  higher  level  interaction,  i.e.,  "intention  estimation" 
as  shown  in  Fig.  2a,  two  dancers'  roles  as  the  leader  and  the 
follower  are  fixed:  the  male  dancer  selects  the  next  step, 
conveys  his  intention  to  the  female  dancer,  who  receives, 
interprets  the  intention  and  follows  the  male  dancer's  lead. 
Because  of  this  explicit  specialization  in  the  higher  level 
interaction,  two  dancers  in  waltz  have  their  names  as 
"leader"  and  "follower." 

However,  on  the  lower  level  interaction  ("coupled 
dynamics"  as  shown  in  Fig.  2b),  we  cannot  rule  out  the 
possibility  of  the  dancers'  role-switching  behavior  during 
dancing.  Although  it  is  difficult  to  quantitatively  evaluate 
one  dancer's  intention  to  lead  (or  to  follow)  in  human-human 


interaction,  to  the  author's  knowledge,  a  human  dancer  can 
hardly  keep  the  role  as  a  pure  leader  (or  follower).  For 
instance,  a  human  leader  may  give  up  leading  if  the 
interaction  force  is  too  large;  a  human  follower  also  may 
"override"  the  lead  if  she  is  struggling  to  keep  balance,  which 
should  have  higher  priority  than  following. 

Intuitively,  the  role-switching  can  be  considered  as  a 
continuous  process,  which  has  been  modeled  by  a  homotopy 
of  maps,  with  a  scalar  parameter  a  E  [0, 1]  used  to  describe 
the  role  between  leader  {a  =  1)  and  follower  (a  =  0)  [38], 
[39],  [40].  At  the  same  time,  the  value  of  a  may  be  a  result  of 
task,  or  interaction  force,  etc.;  modeling  the  evolution  of  a  is  a 
great  challenge.  Introducing  the  role-switching  mechanism 
into  HRI  of  waltz  will  further  complicate  the  system; 
however,  role-switching  has  the  potential  to  contribute  to  a 
more  life-like  interaction.  Therefore,  this  feature  should  be 
included  in  the  future  dance  partner  robot. 

7  Conclusion 

To  develop  a  robot  follower  which  can  dance  waltz  with  a 
human  leader,  the  haptic  interaction  between  the  two 
dancers  is  modeled  and  studied.  The  interaction  is  divided 
into  higher  level  interaction  (step  estimation)  and  lower 
level  interaction  (coupled  body  dynamics). 

As  the  higher  level  interaction  has  been  modeled  with 
HMMs  and  reproduced  in  HRI,  the  focus  of  this  paper  is  the 
lower  level  interaction.  The  human  leader  is  modeled  as  an 
inverted  pendulum.  The  simplified  human  model,  along 
with  an  extended  Kalman  filter,  are  used  in  estimating 
human  leader's  state.  Feed-forward  of  the  leader's  state  is 
utilized  by  the  robot  controller  to  achieve  reduced  interaction 
force.  Two  robot  controllers  (namely,  virtual  force  and 
inverted  pendulum)  are  examined.  The  use  of  the  inverted 
pendulum  controller  is  supported  by  experiments;  the  failure 
of  the  virtual  force  controller  is  explained. 

However,  with  the  large  uncertainty  contained  in  the 
human  model,  it  is  still  a  great  challenge  to  design  a  robot 
controller  which  has  guaranteed  robustness  and  optimal 
performance.  In  addition,  the  leader-follower  role-switching 
in  the  lower  level  interaction  should  also  be  considered. 
Besides,  currently,  we  have  only  modeled  dancers'  motions 
in  sagittal  plane,  with  rotational  motions  still  unstudied. 
These  issues  are  to  be  addressed  in  our  future  work. 
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Abstract — Physical  human-robot  interaction  between  a  hu¬ 
man  leader  and  a  robot  follower  in  waltz  is  studied  in  this 
paper.  The  dancers’  body  dynamics  in  single-support  phase  are 
modeled  as  inverted  pendulums.  On  the  robot  side,  an  ankle 
torque  control  method  is  proposed  and  applied.  The  control  law 
forms  a  time-dependent  vector  field,  which  makes  the  nominal 
orbit  of  the  robot  to  be  an  attractor.  To  physically  interact 
with  human,  the  human  leader’s  state  is  estimated  from  range 
image  data  by  using  an  extended  Kalman  filter.  Parameters  of 
the  robot’s  orbit  are  then  adjusted  according  to  the  leader’s 
estimated  and  predicted  state.  The  proposed  method  is  verified 
by  simulation  results. 

Index  Terms — Physical  human-robot  interaction,  inverted 
pendulum,  attractor,  extended  Kalman  filter. 

I.  Introduction 

In  physical  human-robot  interaction  (pHRI),  a  robot  is 
expected  to  interact  with  a  human  partner  by  utilizing  the 
information  of  the  human  as  well  as  the  physical  connection. 
Usually,  the  robot  is  controlled  at  two  levels.  At  the  higher 
level,  human’s  intention  is  estimated  from  some  accessible 
signals  (e.g.,  force/torque,  EMG,  etc.);  the  estimated  intention 
then  decides  the  next  event  of  the  robot  (intention  estimation, 
or  intention  sensing)  [1]— [3] .  At  the  lower  level,  the  coupled 
dynamics  of  the  human-robot  system  are  modeled,  thus  the 
robot  is  controlled  to  cooperate  with  human  by  utilizing 
model  knowledge  (coupled  dynamics)  [4]— [6] . 

Waltz  is  a  typical  example  of  physical  human-human 
interaction  (pHHI)  in  which  two  dancers  are  acting  as  a 
leader  (usually  the  male  dancer)  and  a  follower  (usually  the 
female  dancer).  Waltz  can  also  be  viewed  at  higher  and  lower 
levels,  as  shown  in  Fig.  1.  At  the  higher  level,  as  waltz  has  a 
“vocabulary”  of  various  kinds  of  dance  steps,  if  the  leader  has 
selected  the  next  step,  the  follower  should  be  able  to  estimate 
the  leader’s  selection.  At  the  lower  level,  as  the  follower’s 
body  dynamics  are  coupled  with  the  leader;  she  has  to  dance 
in  the  context  of  coupled  dynamics. 

To  reproduce  the  pHHI  of  waltz  with  pHRI,  we  developed 
a  mobile  robot  that  plays  the  role  of  the  female  follower. 

*This  work  is  supported  by  the  Asian  Office  of  Aerospace  Research  and 
Development  (AOARD),  Air  Force  Office  of  Scientific  Research  (AFOSR) 
under  grant  number  FA2386-10-1-4126. 


(a)  Intention  estimation  (b)  Coupled  dynamics 

Fig.  1.  Two  levels  of  interactions  in  waltz  [7] 


The  robot  is  expected  to  be  capable  of  estimating  human 
leader’s  next  step  (higher  level)  and  adapting  itself  to  the 
coupled  body  dynamics  (lower  level).  We  hope  that  the 
knowledge  acquired  in  pHRI  may  help  designing  robots  that 
can  physically  interact  with  human  in  more  intuitive  ways. 
Because  the  higher  level  interaction  has  been  studied  in  our 
earlier  work  [8];  therefore,  our  focus  here  is  the  lower  level 
interaction. 

A  proper  model  of  the  dancers’  coupled  body  dynamics 
is  crucial  in  achieving  good  performance  in  pHRI.  In  our 
previous  work  we  modeled  dancers  as  two  linear  inverted 
pendulums  (LIPM,  proposed  in  [9])  connected  by  a  spring 
and  a  damper  [10],  [11].  However,  because  of  LIPM’s 
unstable  orbit,  it  has  large  sensitivity  to  initial  condition  and 
disturbance;  with  the  sensitivity  problem  and  measurement 
noise,  it  is  very  difficult  to  reliably  estimate  and  predict  the 
human  leader’s  state.  To  deal  with  the  problem,  in  this  paper 
we  propose  an  approach  which  makes  the  orbit  to  be  an 
attractor  and  uses  extended  Kalman  filter  for  human  state 
estimation. 

In  Section  II,  models  of  pHRI,  human,  and  robot  are  intro¬ 
duced.  In  Section  III,  the  proposed  method  for  controlling  the 
robot  is  described.  Simulation  results  are  shown  in  Section 
IV  and  conclusions  are  given  in  Section  V. 

II.  Models  of  the  Physical  Interaction 
A.  Model  of  the  Physical  Interaction 

A  schematic  diagram  of  the  lower  level  pHRI  (i.e.,  coupled 
body  dynamics)  in  waltz  is  illustrated  in  Fig.  2.  This  system 
involves  the  interaction  among  three  components:  the  human 
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body,  the  human  arm,  and  the  robot  follower.  Principally, 
the  arm  should  be  considered  as  a  subset  of  the  body; 
however,  as  the  human  arm  is  the  “interface”  between  the 
coupled  systems,  the  interaction  force  /  is  largely  affected 
by  arm  dynamics.  Due  to  its  significant  role  in  the  physical 
interaction,  the  human  arm  dynamics  are  often  separately 
analyzed  [12]. 

As  the  human  body  and  the  human  arm  can  only  be 
commanded  by  human  himself  (intention  of  body  motion, 
and  arm  actuation),  our  goal  is  to  design  the  robot  dynamics 
for  achieving  some  performance  requirements,  e.g.,  spatially 
following  human  ( Xf  tracks  Xi ),  or  reducing  the  interaction 
force  /.  Ideally,  if  models  of  the  human  body  and  the 
arm  are  already  known,  while  accurate  measurement  of 
xi  is  possible,  the  robot  dynamics  can  be  designed  with 
ease.  However,  in  practice  these  prerequisites  can  hardly  be 
satisfied:  precisely  modeling  human  and  measuring  states  are 
still  great  challenges. 

If  we  ignore  the  unknown  human  body  dynamics  and  Xi , 
the  robot  will  be  controlled  in  a  simpler  context:  a  feedback 
loop  which  consists  the  inter-connected  human  arm  and 
robot.  This  feedback  loop  is  a  classic  case  in  pHRI  related 
studies  [4].  Usually  an  assumed  arm  model  with  identified 
parameters  are  used  for  robot  controller  design;  however, 
due  to  the  large  structured/unstructured  uncertainties  in  the 
arm  model,  the  designer  will  tend  to  use  a  worst-case  gain, 
along  with  a  conservative  stability  criterion  (e.g.,  small  gain 
theorem)  to  synthesize  the  robot  controller,  which  leads  to 
the  large  sacrifice  in  performance. 

This  compromise  between  stability  and  performance  can 
be  alleviated  if  more  information  about  the  human  leader  is 
accessible.  An  extreme  case  is  that  if  xi  can  be  accurately 
observed,  we  can  simply  let  Xf  =  Xi  to  realize  human 
following  and  /  reduction.  This  extreme  case,  though  im¬ 
practical,  well  exemplifies  the  effectiveness  of  using  the  feed¬ 
forward  of  Xi  (the  dashed  arrow  in  Fig.  2),  which  has  also 
been  experimentally  validated  in  [13]. 

After  introducing  the  feed-forward  of  Xi ,  we  also  need  an 
assumed  human  body  model,  which  serves  two  purposes: 

1)  The  measured  xi  usually  contains  large  noise/offset.  As 


Arm  actuation 


Fig.  2.  Diagram  of  the  pHRI  model  in  waltz,  xi  and  Xf  are  leader’s  and 
follower’s  spatial  states  (e.g.,  position,  velocity,  etc).  /  is  the  interaction 
force 


Xi  is  the  result  of  human  body  dynamics;  by  using  an 
assumed  human  model,  we  can  design  a  model-based 
filter  to  attenuate  the  measurement  noise; 

2)  The  assumed  human  body  model  can  also  be  used  for 
predicting  xi  in  the  future. 

Therefore,  in  the  subsequent  part  we  will  introduce  the 
human  body  model  which  is  being  used  in  this  paper. 


B.  The  Human  Model 


The  human  leader  generally  keeps  walking  throughout  the 
waltz  dance,  hence  a  human’s  body  model  is  firstly  a  bipedal 
walking  model.  A  simplified  human  model  which  catches  the 
primary  features  of  walking  is  the  inverted  pendulum  model, 
as  given  in  Fig.  3.  This  model  consists  of  a  center  of  mass 
(CoM),  an  extendable  leg  and  an  ankle  joint  actuator,  with 
dynamics  as 


xi 


zi  +g 

Zi 


+ 


Ui 

mtzi 


mi 


(1) 


where  xi  is  the  horizontal  position  of  CoM  with  respect  to 
the  pivot  point,  zi  is  the  height  of  CoM,  mi  is  the  mass 
of  the  inverted  pendulum,  g  is  gravitational  acceleration,  / 
is  external  force,  and  ui  is  ankle  torque.  It  is  difficult  to 
precisely  measure  ui  with  external  sensors,  but  the  repeating 
patterns  of  ui  in  bipedal  walking  can  be  approximated  by 
other  known  states.  Here  we  assume  ui  is  a  function  of  xi, 
xi  and  /,  while  ui(xi,xi,  /)  =  0  if  xi,  xi  and  /  are  all  zero. 
A  linear  approximation  is 


Ui(xhxi,f)  =  k1xi  +  k2xi  +  k3f  (2) 


where  k\  and  k 2  are  the  damping/actuation  coefficients  of  the 
ankle,  ks  stands  for  human’s  reactions  to  the  external  force. 
With  (1)  and  (2),  the  human  model  has  only  one  input  /: 

Xi  =  (  °  1  "]xi  +  [0,b}Tf  (3) 

\  a  1  d2  J 

where  ai  =  (zi+g)/ zt-\-ki/(miZi),a2  =»  k2/(mizi),  and  b  == 
ks /(mizi)  +  1  /mi  are  human  model’s  unknown  parameters. 

The  above  model  can  only  approximate  human’s  body 
dynamics  in  single-support  phase,  besides  which  there  is 
another  double- support  phase.  In  double- support  phase,  hu¬ 
man  is  almost  a  fully  actuated  system  and  corresponds  to  a 
completely  different  model  instead  of  (1)  and  (2). 


Fig.  3.  Simplified  human  body  dynamics  with  an  inverted  pendulum  model 
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The  transition  between  the  single- support  and  the  double¬ 
support  phases  are  foot  landings,  i.e.,  when  the  swing  foot 
lands  on  ground.  At  foot  landings,  xi  is  instantaneously 
reset  with  a  new  value,  hence  the  time  history  of  xi  and 
xi  formulates  a  set  of  orbits  in  phase  plot.  At  the  same  time, 
since  for  a  human  leader  in  waltz  this  reset  is  synchronized 
with  music  beats  (denoted  by  { ti }),  the  orbit  of  xi  and  xi  is 
time-dependent,  i.e.,  xi  is  reset  when  t  E  {£/},  as  shown  in 
Fig.  4. 

Compared  with  the  gravity-dominated  body  motions  in 
single- support  phase,  it  is  difficult  to  model  human’s  behavior 
in  double- support  phase.  However,  for  a  robot  which  is 
physically  interacting  with  human,  it  is  necessary  to  deal  with 
the  two  phases  with  distinctive  dynamics.  A  straightforward 
solution  is  using  one  conservative  robot  model  that  can  work 
stably  in  both  phases,  but  this  will  usually  cause  degradations 
in  performance.  As  we  have  some  information  about  the 
dynamics  in  single- support  phase,  those  information  should 
be  exploited  to  enhance  interaction  performance. 

C.  The  Robot  Model 

Generally,  for  the  system  described  in  Fig.  2,  there  are 
infinite  candidate  robot  models  that  can  be  implemented  to 
control  the  pHRI.  Because  our  purpose  is  to  realize  a  life¬ 
like  interaction  as  in  the  case  of  pHHI,  a  human-like  robot 
model  is  preferred.  The  model  used  here  is  linear  inverted 
pendulum  (LIPM)  [9],  [14],  which  has  simplified  dynamics 
as 

Xf  =  ( 9 / Zf)xf  +Uf/  ( mfZf )  +  f/mf  (4) 

where  Xf  is  the  robot  follower’s  CoM  position  with  respect 
to  the  pivot  point,  Zf  is  the  height  of  CoM,  is  follower’s 
mass,  and  Uf  is  the  ankle  torque. 

For  simplicity,  this  model  does  not  contain  double- support 
phase.  In  addition,  values  of  the  reset  Xf  at  k- th  foot  landing 
moments  (which  are  also  the  music  beat  moments  in  waltz, 
denoted  by  tf(k ))  are  controlled  by  a  balance  controller  to 
guarantee  the  CoM  velocity  at  moment  tf(k  +  1).  Consider 
that  the  period  of  beat  moments  has  a  nominal  value  Tp, 
i.e.,  Tp  =  tf(k  +  1)  —  tf(k),  the  balance  controller  resets 
Xf  at  moment  tf(k)  by  using  the  following  rule  such  that 
Xf(tf(k-\- 1))  =  Vd(tf(k+1))  can  be  achieved  (given  Uf(t)  = 


0  when  tf(k)  <t<  tf(k  +  1))  [10], 

x+{tf{k))  =  -^Xf  ( tf(k ))  +  ^ vd  ( tf(k  +  1))  (5) 

where  Vd(tf(k  +  1))  is  the  desired  velocity  at  tf(k  +  1). 
r  =  yj z f  / g,  C  =  cosh(Tp/r)  and  S  =  sinh (Tp/r). 

With  the  LIPM  dynamics  described  in  (4)  and  the  balance 
controller  in  (5),  the  time-dependent  orbit  of  Xf  =  [xf,  Xf]T 
is  illustrated  in  Fig.  5.  If  Uf  =  0,  the  trajectory  of  the  single¬ 
support  phase  is  a  hyperbola  with  an  invariant  orbital  energy 

[9]. 


III.  Control  of  the  Robot 


A.  Orbit  Control  of  the  Robot 


Although  the  robot  can  be  balanced  by  resetting  x  at 
beat  moments  {tf}  using  (5),  the  control  rule  is  intermittent 
and  cannot  cover  the  moments  throughout  the  motion;  if 
disturbance  is  introduced  during  single- support  phase,  tra¬ 
jectory  will  deviate  from  the  nominal  orbit;  as  LIPM  is 
linear  and  unstable,  this  deviation  will  increase  exponentially. 
Therefore,  additional  control  by  using  the  ankle  torque  Uf  is 
necessary. 

Ignoring  the  interaction  force  /,  system  (4)  can  be  written 


as 


*/ 


g/Zf  j  ^  */  +  [0.  llimfZf)\Tuf  W 


As  the  system  is  controllable,  usually  we  can  use  a  state- 
feedback  gain  to  replace  the  pendulum’s  original  dynamics 
with  an  artificial  one  (with  designed  poles).  However,  this  re¬ 
quires  large  ankle  torque,  which  is  impossible  since  human’s 
ankle  torque  is  limited  by  the  size  of  supporting  foot. 

For  bipedal  walking,  this  control  can  be  realized  by  turning 
the  nominal  orbit  into  an  attractor.  For  a  state-dependent 
system,  a  vector  field  near  the  nominal  orbit  can  be  designed 
to  “attract”  neighboring  states  onto  the  orbit  [15],  as  shown 
in  Fig.  6(a). 

Similarly,  it  is  easy  to  design  an  vector  field  near  the 
robot’s  nominal  orbit  according  to  orbital  energy,  such  that 
solutions  with  arbitrary  initial  conditions  can  converge  to 
the  orbit.  However,  as  our  system  is  time  dependent  (x 
is  reset  only  at  beat  moments  {tf})  while  the  LIPM  is 
expected  to  reach  the  desired  velocity  Vd  when  t  E  {tf}, 
convergence  to  the  nominal  orbit  may  lead  to  the  violation  of 
this  requirement  (the  dotted  dash  curve  in  Fig.  6(b)).  Instead, 


Fig.  4.  The  orbit  in  walking 


Fig.  5.  Orbit  of  the  robot 
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(a)  State-dependent  orbit 


(b)  Time-dependent  orbit 


Fig.  6.  Designing  attractors  for  two  types  of  orbits.  Arrows  are  vector  fields; 
dotted  dashes  are  solution  trajectories  for  state-dependent  orbits;  dashed 
curve  in  (b)  is  the  solution  for  time-dependent  orbits 


a  desired  solution  may  deviate  from  the  nominal  orbit  (the 
dashed  curve  in  Fig.  6(b)),  depending  on  the  current  time. 
In  another  word,  to  deal  with  the  time-dependent  system,  the 
vector  field  should  also  be  time-dependent  (more  specifically, 
time- state-dependent).  Design  of  the  vector  field  will  be 
introduced  in  the  following  part. 

The  robot  is  controlled  in  discrete  manner  with  sampling 
period  ts,  the  robot  dynamics  is 


Xf(k  +  1)  =  ArXf(k)  +  BrUf(k )  (7) 


where  Ar  and  Br  are  discrete  forms  of  the  matrices  appeared 
in  (6). 

The  state  at  time  step  N  is 

N—l 

xf(N)  =  A?-kxf(h)  +  (8) 

i=k 

Assume  there  exists  a  sequence  Uf(k)...Uf(N  —  l)  which 
makes  Xf(N )  =  x*j,  where  x*j  is  the  desired  robot  state  at 
time  step  N.  Define  state  error  at  time  step  k  as 

N-l 

e(k)  =  x*f-  A^~kxf(k)  =  ]T  (A^~l~1BrUf(i))  (9) 

i=k 

e(k)  is  the  difference  between  the  desired  state  and  a 
predicted  state,  given  all  future  efforts,  i.e.,  Uf(k)...uf(N- 

1)  =  0.  The  evolution  of  e(k)  is 

e(k  +  l)  =  e(k)-A^-k~1Bruf(k)  (10) 


One  may  expect  to  use  the  feedback  of  e(k)  to  build  a 
stable  system,  in  which  e(k)  asymptotically  converges  to  0 
as  k  -A  N.  Unfortunately,  because  (10)  is  uncontrollable, 
e(k)  cannot  be  eliminated.  As  we  are  only  concerned  about 
velocity  errors,  here  only  e(k)  is  considered.  The  control  law 
of  Uf  is 

x*r-[A?-kXf(k)\{21) 

Uf(k)  r  AAf_k_  i  ^  ii  (11) 


7  [AH 


J  (2,1) 


where  [M]^^  denotes  matrix  M's  entry  on  row  i  and 
column  j.  7  >  1  is  a  scalar  used  to  control  the  convergence 
speed.  With  Uf  being  controlled  by  (11),  we  have 


e(k  +  1)  =  (1  -  l/7)e(fc)  (12) 


which  suggests  the  exponentially  diminishing  error  of  veloc¬ 
ity. 

The  method  presented  in  (1 1)  fully  utilizes  LIPM’s  original 
dynamics  and  results  in  an  efficient  control  law.  When  desired 
velocity  Vd  and  beat  moments  {£/}  are  given,  an  attractive 
orbit  is  defined.  At  the  same  time,  to  physically  interact  with 
the  human  leader,  the  orbit  parameters  Vd  and  {tf}  should  be 
adapted  to  the  leader’s  orbit.  Because  these  parameters  are 
adjusted  according  to  human  leader’s  state  (the  feed-forward 
path  in  Fig.  2),  in  the  following  we  will  introduce  the  method 
of  estimating  and  predicting  the  human  leader’s  state. 

B.  Estimating  and  Predicting  the  Human  Leader's  State 

In  single- support  phase,  human  leader’s  motion  is  largely 
determined  by  gravity,  this  makes  leader’s  state  Xf  easier  to 
estimate  and  predict.  To  obtain  the  human  leader’s  state,  we 
installed  two  laser  range  finders  (LRF)  on  the  robot  to  detect 
human’s  waist  and  ankle  positions,  with  which  the  state  xi 
can  be  inferred,  as  shown  in  Fig.  7. 

However,  there  are  also  several  difficulties  in  the  state 
estimation/prediction  task,  namely 

1)  The  human  model  given  in  (1)  is  not  the  exact  model 
of  human’s  body  dynamics; 

2)  Model  parameters  are  unknown; 

3)  The  noise  and  offsets  contained  in  the  measurements 
from  the  LRFs. 

With  (3),  the  listed  difficulties  can  be  reformulated  as  a 
system  with  uncertainties,  process  noise,  and  measurement 
noise: 

Xi  =  f  °  1  )  Xi  +  [0,6]t/+  [w1,w2]t 

\  al  a2  J 

y  =  [xi+5,±i]t  +  [v  i,v2]t  (13) 

where  a\,  a^,  and  b  are  unknown  model  parameters,  S  is  the 
unknown  offset  of  measured  CoM.  w±,  are  the  process 
noises;  v\  and  v 2  are  measurement  noises. 

By  introducing  an  extended  state  xe ,  which  includes  the 
unknown  parameters,  we  have 

xe  =  [xi,  Xi,a1,a2,  b,  <)]r  (14) 


LRF 


orce  sensor 


Fig.  7.  Two  LRFs  are  installed  on  the  robot  for  detecting  human’s  waist 
and  ankle  positions 
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then  the  system  defined  in  (13)  turns  to  be  nonlinear,  with 

xe  =  ge{xe,f)  +  [wi,w2]t 
y  =  he(xe) +  [v1,v2]T  (15) 

The  nonlinear  system  in  (15)  is  firstly  discretized,  then  its 
state  is  estimated  by  using  an  discrete  extended  Kalman  filter 
(EKF).  Details  of  the  EKF  will  not  be  discussed  here.  The 
estimated  state  and  model  parameters  can  also  be  used  to 
predict  the  future  state. 

Since  in  waltz  two  dancer’s  motions  are  synchronized, 
the  other  necessary  information  is  the  human  leader’s  beat 
moments  {£/}  and  support  phases  (single-support  or  double¬ 
support).  These  are  also  inferred  from  LRF  data  by  detecting 
foot  landing  moments,  while  details  can  be  found  in  [11]. 

C.  Following  the  Human  Leader 

With  the  feed- forward  of  the  human  leader’s  estimated 
state,  we  can  adapt  the  robot’s  orbit  parameters  to  realize 
a  coupling  in  which  the  robot  can  well  follow  the  human 
leader  in  a  life-like  way. 

In  single-support  phase,  if  we  are  at  time  step  k  while 
the  next  beat  moment  is  on  N,  we  first  estimate  the  human 
leader’s  state/parameters  xe(k )  with  EKF.  From  the  estima¬ 
tion  xe(k ),  human  leader’s  velocity  at  N  can  be  predicted 
(we  denote  this  prediction  by  D/(k)).  Finally,  the  predicted 
velocity  vi(k )  substitutes  x*  in  (11)  and  Uf(k )  is  then 
obtained. 

In  double- support  phase,  as  the  human  model  is  unclear, 
we  have  Uf  =  0.  At  beat  moments  {£/},  Xf  is  reset  according 
to  (5). 

IV.  Simulations 

A.  Control  of  the  Robot 

In  simulations,  parameters  of  the  robot  follower  are  m/  = 
45  kg,  Zf  =  0.9  m;  period  of  beat  moments  is  Tp  =  0.75  s. 

The  ankle  torque  control  in  (11)  actually  generates  a  time- 
dependent  vector  field.  To  visualize  this  vector  field,  the  time 
axis,  ranging  from  0  to  Tp ,  is  included  as  one  dimension, 
result  is  shown  in  Fig.  8.  We  can  see  that  the  solution  curves 
with  different  initial  x  f  and  t  have  been  attracted  by  the  set 
defined  by  t  £  {£/}  and  Xf  =  Vd- 

Given  Vd  and  {£/},  the  vector  field  of  (11)  and  the  x- 
resetting  rule  of  (5)  are  defined,  yielding  a  stable  and  periodic 
orbit  of  the  robot,  as  shown  in  Fig.  9. 

B.  The  Physical  Interaction 

To  simulate  the  interaction  between  two  dancers,  we  mod¬ 
eled  them  as  two  inverted  pendulums  connected  by  a  spring 
and  a  damper.  Parameters  of  the  simulation  are  mi  =  70  kg, 
zi  =  lm.  The  connection  is  assumed  to  have  stiffness 
of  100  N/m  and  damping  of  30Ns/m.  Timing  errors  are 
considered  in  two  dancer’s  synchronization  with  timing  error 
0.1  s.  Measurements  of  the  FRFs  are  assume  to  have  ±0.05  m 


Fig.  8.  The  time-dependent  vector  field  generated  by  (11).  v d  is  set  to  1. 
The  cones  are  vectors  of  flow  directions;  the  curves  are  integrated  streams 
of  solutions 


Fig.  9.  Solution  orbit  of  the  robot 

white  noise  and  —0.05  m  bias  in  xu  and  ±0.2  m/s  white 
noise  in  x\.  Results  of  simulation  are  shown  in  Fig.  10. 

Due  to  model  uncertainties  and  measurement  noise/bias, 
the  predicted  v\  is  quite  inaccurate  at  the  beginning;  however, 
the  prediction  error  converges  as  the  time  is  approaching  the 
next  beat  moment,  as  demonstrated  in  Fig.  10(a).  According 
to  Fig.  10(b)  and  Fig.  10(c),  the  two  dancer’s  orbits  are 
coupled  with  reduced  interaction  force  (compared  with  the 
results  of  [10],  [11]).  The  simulated  ankle  torque  Uf  is  also 
kept  within  a  reasonable  range  (Fig.  10(d)).  Our  proposed 
method  is  hence  supported  by  the  simulation  results  in  Fig. 
10. 

V.  Conclusion 

In  this  paper,  we  first  introduce  a  pHRI  model,  which 
implies  the  necessity  of  designing  the  robot  dynamics  model 
as  well  as  implementing  an  assumed  model  of  the  human 
leader.  We  approximate  human’s  body  dynamics  in  single¬ 
support  phase  with  an  inverted  pendulum,  while  the  robot 
is  controlled  to  emulate  the  dynamics  of  an  FIPM.  In  the 
robot’s  single- support  phase,  an  ankle  torque  control  method 
is  proposed.  The  ankle  torque  control  forms  a  time-dependent 
vector  field,  turning  the  nominal  orbit  of  the  robot  to  be  an 
attractor.  To  physically  interact  with  human,  parameters  of 
the  attractor  are  adjusted  according  to  human  leader’s  state 
obtained  from  FRFs.  The  proposed  method  is  validated  by 
simulations. 

However,  the  current  model  only  considers  translational 
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1.2 


(c)  Interaction  force 


(d)  Ankle  torque  of  robot 


Fig.  10.  Simulated  pHRI  with  the  proposed  robot  controller.  The  human 
leader  is  supposed  to  have  varying  vj  with  Vd(ti(l)). .  .Vd{ti(9))  be¬ 
ing  {1,  0.3,  0.1, 1,  0.3,  0.1, 1,  0.3,  0.1}(m/s),  respectively.  Thin  and  thick 
solid  curves  in  (a)  and  (b)  are  human  and  robot’s  states.  Dashed  curve  in 
(a)  is  the  predicted  velocity  vi 


[9]  S.  Kajita  and  K.  Tani,  “Study  of  dynamic  biped  locomotion  on  rugged 
terrain — theory  and  basic  experiment,”  Advanced  Robotics,  1991. 

’ Robots  in  Unstructured  Environments’ ,  91  ICAR.,  Fifth  International 
Conference  on,  pp.  741-746  vol.l,  1991. 

[10]  H.  Wang  and  K.  Kosuge,  “Towards  an  understanding  of  dancers’ 
coupled  body  dynamics  for  waltz,”  Proceedings  of  the  201 1  IEEE/RSJ 
International  Conference  on  Intelligent  Robots  and  Systems  (IROS), 
pp.  2008-2013,  2011. 

[11]  - ,  “Understanding  and  reproducing  waltz  dancers’  body  dynamics 

in  physical  human-robot  interactio,”  Proceedings  of  the  2012  IEEE 
International  Conference  on  Robotics  and  Automation  (ICRA),  to 
appear. 

[12]  F.  Mobasser  and  K.  Hashtrudi-Zaad,  “A  method  for  online  estimation 
of  human  arm  dynamics,”  in  Proceedings  of  the  2006  IEEE  Interna¬ 
tional  Conference  of  Engineering  in  Medicine  and  Biology  Society, 
2006,  pp.  2412-2416. 

[13]  N.  Jarrasse,  J.  Paik,  V.  Pasqui,  and  G.  Morel,  “How  can  human  motion 
prediction  increase  transparency?”  in  Proceedings  of  the  2008  IEEE 
International  Conference  on  Robotics  and  Automation  (ICRA),  2008, 
pp.  2134-2139. 

[14]  S.  Kajita,  F.  Kanehiro,  K.  Kaneko,  K.  Yokoi,  and  H.  Hirukawa, 
“The  3D  linear  inverted  pendulum  mode:  a  simple  modeling  for  a 
biped  walking  pattern  generation,”  Proceedings  of  the  2001  IEEE/RSJ 
International  Conference  on  Intelligent  Robots  and  Systems  (IROS), 
no.  4,  pp.  239-246,  2001. 

[15]  M.  Okada  and  K.  Murakami,  “Robot  communication  principal  by 
motion  synchronization  using  orbit  attractor,”  in  Proceedings  of  the 
2007  IEEE  International  Conference  on  Robotics  and  Automation 
(ICRA),  2007,  pp.  2564-2569. 


motions  in  dancers’  sagittal  plane,  leaving  rotational  motions 
(body  spins  and  turns)  unstudied.  In  addition,  experimental 
validations  are  still  to  be  finished.  These  issues  will  be 
included  in  our  future  work. 
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Understanding  and  Reproducing  Waltz  Dancers’  Body  Dynamics  in 

Physical  Human-Robot  Interaction 

Hongbo  Wang  and  Kazuhiro  Kosuge 


Abstract — A  pair  of  spring-damper-connected  inverted  pen¬ 
dulums  are  introduced  to  model  two  dancers’  body  dynamics 
in  physical  interaction.  When  timing  errors  are  included  in  the 
model,  condition  for  poly- quadratic  stability  is  implemented  to 
examine  the  system.  With  two  laser  ranger  finders  installed 
on  the  robot  for  measuring  human  dancer’s  states,  a  state- 
feedback-based  method  is  proposed  to  minimize  the  interaction 
force;  because  in  simulation  the  theoretically  optimal  feedback 
gain  is  sensitive  to  measurement  noise,  another  set  of  empirical 
gains  are  used  and  proved  to  be  effective  in  experiments. 

Index  Terms —  Physical  human-robot  interaction,  dance  part¬ 
ner  robot,  linear  inverted  pendulum,  poly- quadratic  stability, 
laser  ranger  finder. 

I.  Introduction 

Physical  human-robot  interaction  (pHRI)  involves  the  cou¬ 
pled  system  of  human  and  robot.  By  utilizing  the  information 
through  the  coupling,  the  robot  is  able  to  interact  with  human 
in  expected  ways.  To  achieve  a  well-coordinated  interaction, 
the  robot  usually  maintains  models  at  two  levels.  At  the 
higher  level,  the  correlation  between  human’s  intentions  and 
accessible  signals  (e.g.,  force/torque,  EMG,  etc.)  is  modeled 
for  the  purpose  of  realizing  human  intention  estimation  (or 
intention  sensing)  [1]— [3] .  At  the  lower  level,  as  pHRI  is 
constrained  by  the  physical  laws  that  govern  human’s  and 
robot’s  motions,  dynamics  of  the  human- involved  system 
should  also  be  modeled;  hence  the  robot  can  properly  affect 
the  coupled  dynamics  by  utilizing  model  knowledge  [4]— [6] . 

Waltz  is  a  typical  example  of  demonstrating  human’s 
capabilities  in  physical  human-human  interaction  (pHHI), 
which  can  also  be  viewed  at  higher  and  lower  levels.  Waltz 
has  a  “vocabulary”  of  various  types  of  steps,  while  a  dance 
consists  of  a  series  of  temporally  concatenated  steps.  In  a 
social  setting,  where  the  male  dancer  (leader)  selects  the  next 
step  in  improvised  ways,  the  female  dancer  (follower)  can 
estimate  the  leader’s  next  step  by  using  haptic  signals;  this  is 
higher  level  interaction.  At  the  same  time,  whether  the  next 
step  is  known  or  unknown  to  the  follower,  she  is  still  well- 
adapted  to  the  coupled  body  dynamics,  as  well  as  keeping 
her  own  balance;  this  is  lower  level  interaction. 

The  goal  of  our  research  is  to  reproduce  the  pHHI  of  waltz 
with  pHRI,  in  which  a  mobile  robot  plays  the  follower’s  role, 
being  capable  of  estimating  human  leader’s  next  step  (higher 
level)  and  adapting  itself  to  the  coupled  body  dynamics 
(lower  level).  We  hope  that  more  knowledge  acquired  from 
this  pHRI  may  help  designing  robots  that  can  physically 
interact  with  human  in  more  intuitive  ways.  In  one  earlier 
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work,  the  higher  level  interaction  has  been  studied  [7]; 
therefore,  our  current  focus  is  investigating  the  lower  level 
interaction. 

On  the  lower  level  interaction,  one  of  our  previous  work 
modeled  the  robot  as  a  free  mass  following  various  prede¬ 
fined  dance  trajectories,  whose  parameters  can  be  adjusted  by 
learning  [8].  In  another  work  we  used  a  pair  of  connected 
inverted  pendulums  as  a  more  accurate  system  model  [9]. 
By  assuming  the  two  dancers’  motions  are  synchronized,  we 
analyzed  stability  of  the  system,  while  interaction  force  was 
reduced  with  gradient  descent  method  [10]. 

In  waltz,  two  dancers  are  roughly  synchronized  with  the 
help  of  music  beats;  however,  timing  errors  between  the  two 
dancers  can  frequently  occur.  Due  to  the  inevitable  timing 
errors,  our  assumption  on  the  precisely  synchronized  motion 
is  too  strong;  therefore,  in  this  paper,  we  will  remove  this 
strong  assumption  and  include  timing  errors  in  the  model 
description.  In  addition,  because  the  gradient  descent  method 
we  used  was  slow  and  often  sensitive  to  step  size,  in  this 
paper  we  also  propose  a  method  which  reduces  interaction 
force  by  utilizing  more  human  dancer’s  information  from 
additional  sensors  (two  laser  range  finders). 

Two  dancers’  motions  in  sagittal  plane  are  analyzed  with 
the  same  model  proposed  in  [9],  [10];  the  limitation  of 
this  model  is  that  it  only  accounts  for  human’s  translational 
motions  that  can  be  treated  as  two  independent  components 
in  sagittal  and  frontal  plane.  Accordingly,  for  the  various 
dance  steps  in  waltz,  only  a  few  steps  without  body  rotation 
(e.g.,  closed  changes)  can  be  the  subjects  of  our  study. 
Therefore,  we  are  actually  modeling  two  dancers’  “coupled 
walking”  instead  of  the  true  waltz  dancing.  Clearly,  those 
very  simple  cases  are  still  far  from  the  true  dances  that  we 
expect  the  robot  to  do;  however,  they  offer  a  good  start  point 
from  which  we  can  explore  physical  human-robot  interaction 
(pHRI),  such  as  the  one-dimensional  cases  in  [6],  [11],  [12]. 

In  Section  II,  the  system  model  is  introduced.  In  Section 
III,  stability  of  the  two-dancer  system  and  the  method  for 
minimizing  interaction  force  are  analyzed.  Simulation  and 
experiment  results  are  shown  in  Section  IV  and  conclusions 
are  given  in  Section  V. 

II.  System  Model 

A.  Simplified  Model  for  Single  Dancer's  Body  Dynamics 

Linear  inverted  pendulum  (LIPM)  is  a  largely  simplified 
model  for  biped  walking  systems  [13].  If  we  consider  the 
legs  as  massless  while  applying  some  constraints  on  CoM’s 
(center  of  mass)  vertical  motion,  we  can  simplify  a  walking 
system  as  Fig.  1(a)  into  an  LIPM  as  in  Fig.  1(b)  [9].  The 
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linear  dynamics  of  the  LIPM  in  x  direction  is  [13]: 

x  =  ( g / z)x  +  (1  /(mz))uank  (1) 

where  x  is  the  position  of  CoM  with  respect  to  LIPM’s  pivot 
point,  2  is  the  height  of  CoM,  g  is  gravity  acceleration,  m 
is  mass  of  the  body,  and  uan k  is  ankle  torque.  Since  uan k 
is  limited  in  value,  it  is  often  used  for  disturbance  rejection 
rather  than  as  a  major  source  of  control  input,  hence  (1)  can 
also  be  written  in  homogeneous  form  without  the  uar±  term. 

Due  to  the  instability  of  LIPM  (as  can  be  seen  in  (1));  to 
have  a  working  model,  we  also  need  a  balance  controller, 
which  can  intermittently  set  new  values  for  x,  i.e.,  this 
inverted  pendulum  can  instantaneously  relocate  pivot  point 
to  keep  balance.  The  LIPM  along  with  its  controller  can  be 
viewed  as  an  impulsive  dynamical  system: 

(  X  =  ( g/z)x ,  t<£{tk} 

\  x+'  =  w(x~,x),  t  e  {tk} 

where  {tk}  is  the  set  of  moments  when  pivot  point  in¬ 
stantaneously  reaches  a  new  position.  Since  in  waltz  {tk} 
corresponds  to  the  moments  of  music  beat,  hereafter  we  will 
call  {tk}  beat  moments ,  while  x~ ,  x+  are  x  before  and  after 
a  beat  moment. 

The  function  w  in  (2)  is  the  balance  controller.  There  are 
many  methods  to  obtain  w.  In  [10]  we  listed  3  candidates 
(an  energy  controller,  a  velocity  controller,  and  a  hybrid 
version  of  the  former  two)  and  finally  selected  the  hybrid  one. 
However,  in  this  paper  we  will  use  the  velocity  controller. 
The  reason  is  that  when  there  is  no  timing  error  in  beat 
moments  (i.e.,  tk  —  tk-i  is  constant  for  all  fc),  both  the 
velocity  controller  and  the  hybrid  one  can  direct  the  system 
to  desired  velocities;  when  there  is  timing  error,  we  can 
also  find  an  upper  bound  of  errors  when  using  the  velocity 
controller.  Below  is  the  explanation: 

Consider  that  the  period  of  beat  moments  has  a  nominal 
value  Tp ,  tk  —  tk- i  may  be  different  from  Tp,  but  the 
controller  still  takes  Tp  as  a  constant  and  known  parameter. 
The  velocity  controller  is 

x+  =  -(tC  /  S)x(tk)  +  (r/S)vd(tk+ 1)  (3) 

where  vd(tk+i)  is  a  reference  input  which  represents  the 
desired  velocity  at  tk+ 1-  r  =  yfzjg.  And  C  =  cosh (Tp/r) 
and  S  =  sinh (Tp/r)  are  parameters  containing  the  constant 
Tp. 

Let  tk+1  be  the  instant  just  before  (. k  +  l)th  beat  moment, 
while  tk+i  —  tk  =  Tp  +  STp  due  to  the  existence  of  timing 


wr  vv 


(a)  Simplified  human  walking  model  (b)  Linear  inverted  pendulum 
Fig.  1.  LIPM  as  a  simplified  model  [9] 


error  STp.  Assuming  LIPM’s  velocity  is  not  changed  before 
and  after  pivot  relocation,  i.e.,  x(tk)  =  x(tk)  =  x(tj. c)  for 
all  fc,  and  by  solving  (2),  we  have: 

x(tk+ 1)  =  -ol!  {STp)x(tk)  +  a2(STp)vd(tk+ i)  (4) 

where  a\(5Tp)  and  ct2(STp)  are  two  scalar  functions  of  STP 
with  ai(STp)  =  smh(STp / r) /  smh(Tp / r)  and  a 2 (STP)  = 
sinh  ((Tp  +  STp)/t)  /  sinh(Tp/r).  In  waltz  STP  should  be 
smaller  than  Tp  (at  least  smaller  than  Tp/ 2,  otherwise  the  two 
dancers  would  be  in  opposite  phases),  thus  \a\(5Tp)\  <  1, 
and  both  (STp)  and  ct2(STp)  are  bounded. 

Defining  the  error  of  the  velocity  controller  as  e(t/c)  = 
x(tk)  -  vd(tk),  it  yields 

e(tk+i)  =  -a1(STp)e(tk)  +  j(STp)vd(tk+1)  (5) 

with  7 (STp)  =  a2(STp)  —  ai(STp)  —  1.  This  7 (STP)  is  also 
bounded.  Let  ol\  >  \ai(STp)\,  6l\  <  1  and  7  >  \^(STP)\  be 

two  upper  bounds,  then 

|e(tfc+i)|  <  OLi\e{tk)\  +7bd(4+i)|  (6) 

If  vd(tk)  has  a  constant  value  vd,  the  maximum  error 
bound  is  |e(to)|  or  7|u^|/(l  —  <7i),  depending  on  which  is 
larger. 

Based  on  the  above  analysis,  in  the  subsequent  part  we 
will  use  LIPM  along  with  the  velocity  controller  as  the  single 
dancer’s  model. 

B.  Two-LIPM  Model  for  Dancers  in  Physical  Interaction 

To  model  the  dynamics  of  two  dancers  in  physical  con¬ 
nection,  we  use  a  pair  of  LIPMs  connected  by  a  spring  and 
a  damper,  as  shown  in  Fig.  2  [10].  Let  x\  and  x ^  be  the 
CoM  positions  of  the  leader  and  the  follower,  and  pf  and  p ^ 
be  their  pivot  positions,  all  with  respect  to  the  global  frame. 
The  state  vector  x  is  defined  as 

x  =  [xi,xi,xf,Xf,q]T  (7) 

where  x\  =  x9{  —  pf,  Xf  =  x9^  —  p9p  which  are  the  leader’s 

and  the  follower’s  relative  positions  of  CoM  with  respect  to 
their  own  pivot  points,  q  =  x9^  —  xg{  —  do,  with  do  being 
the  spring’s  natural  length.  Let  kc  and  dc  be  constants  of 
the  spring  and  the  damper,  the  system  dynamics  of  the  two- 
LIPM  system  is: 

(  x  =  Ax,  t<£  {tlk}u{t{} 

<  x+  =  Hix~+Bivld,  te{tlk }  (8) 

l  *+  =  Hfx~  +  Bfvd,  t  e  {t{} 


Fig.  2.  Two  connected  LIPMs  as  two  dancers’  model  [10] 
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where  {tlk}  and  {tk}  are  the  leader’s  and  the  follower’s 
respective  beat  moments.  Matrix  A  is 
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with  zu  Zf  being  the  leader’s  and  the  follower’s  CoM  heights 
and  mi,  rrif  being  their  body  masses.  Hi,  Hf,  Bi,  and  Bf 
are  the  matrix  forms  of  (3),  with 
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and  Bt  =  [ti/ Si,  0, 0, 0, 0]r,  Bf  =  [0, 0,  rf/Sf,  0, 0]T. 
Hi  and  Hf  commute:  HjHf  =  Hf  Hj  =  H .  Symbols 
like  tij,  Cij  and  Sij  are  defined  similarly  as  in  (3). 
Also,  as  a  system  of  two  physically  interacting  LIPMs,  their 
interaction  force  is  denoted  by  /,  with  f  —  cx,  where 
c  —  [0,  dc,  0,  dc,  kci\' 


III.  Analysis  on  Stability  and  Optimal 
Interaction 

A.  Stability  of  the  Two-LIPM  System  Considering  Synchro¬ 
nization  Error 

A  simplified  version  of  system  (8)  was  analyzed  in  [10]. 
By  assuming  two  dancers’  motions  are  precisely  synchro¬ 
nized,  i.e.,  tlk  =  tk  =  tk  for  all  k,  system  (8)  degenerates 
into  an  equivalent  discrete  LTI  system: 

3'(fk+l)  =  H AdX(fk)  BiV difk-\- 1)  H-  B fvfj(tk Tl)  (12) 

Ad  is  the  discrete  form  of  A  with  Ad  =  eATp .  When  vld  and 
vd  are  constant,  exponential  stability  of  system  (12)  can  be 
straightforwardly  examined  by  checking  the  spectral  radius 
of  HAd  (denoted  by  p(HAd )):  if  p(HAd)  <  1,  the  system 
is  stable. 

The  above  method  relies  on  the  assumed  synchronized 
motion  of  the  two  dancers,  while  in  real  applications  their 
timing  errors  (or  synchronization  error)  usually  exist.  In  this 
part  we  will  consider  timing  error  as  a  part  of  the  model  and 
analyze  the  affected  stability. 

Consider  the  leader’s  and  the  follower’s  kth  beat  moment 
tlk  and  tk,  as  shown  in  Figure.  3.  Because  it  is  pHRI  in 
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Fig.  3.  Timing  errors  of  the  leader  and  the  follower 


which  the  follower  is  a  robot,  without  loss  of  generality,  we 
can  have  >  A  for  all  k.  In  another  word,  we  assume  the 
robot  has  some  sensors  for  detecting  tlk ;  the  detected  moment 
cannot  be  exactly  the  real  tlk,  but  by  adjusting  the  time  delay, 
we  can  keep  tk  always  in  the  “right”  neighborhood  of  tlk, 

i.e.,  _ 

0  <  tk  —  tlk  =  Stk  <  5tf,Vk  (13) 

where  the  positive  Stk  is  the  follower’s  error  in  following 
the  leader’s  timing,  and  Stf  is  its  upper  bound. 

Unlike  the  robot  follower,  the  human  leader’s  motions 
are  synchronized  with  music  beats.  Ideally,  there  should  be 
tlk+1  —  tlk  =  Tp,  but  again,  due  to  the  inevitable  timing  error 
of  human,  along  with  the  robot’s  following  error,  we  have 

-Sti/2  <  Stlk+ 1  =  4+1  —  t{  —  Tp  <  Sti/2,\/k  (14) 

Compared  with  the  previous  strong  assumption  of  syn¬ 
chronized  motion,  the  new  boundedness  assumptions,  i.e., 
0  <  stfk  <  Stf  and  —Sti/ 2  <  5tlk  <  Sti/ 2  are  reasonably 
weak:  if  Stk  or  K  is  large,  it  would  be  very  difficult  to 
continue  the  dance  even  for  two  human  dancers. 

Now  we  can  examine  one  cycle  of  the  system  from  the 
moment  tk+  to  tk^v  In  this  cycle  the  system  has  four  times 
of  transitions,  namely 

1)  From  tk+  to  tlk+1,  in  which  there  is  no  beat  moment; 
system  dynamics  is  continuous; 

2)  From  tlk+1  to  tlk+1,  the  leader’s  beat  moment,  impul¬ 
sive  dynamics; 

3)  From  tlk+1  to  tk+l9  a  short  period  of  continuous 
dynamics; 

-P _  -P  | 

4)  From  tk+1  to  the  follower’s  beat  moment,  im¬ 

pulsive  dynamics. 

According  to  the  above  sequence  of  transitions  and  (8), 
(13),  (14),  we  can  write 

*(*£+!)  =  HfeASt£+iHieA(Tv+Stl’‘+i'>x(t{+) 

+HfeA<+>  Biv\  +  Bfvfd  (15) 

Similar  to  (12),  (15)  can  also  be  viewed  as  a  discrete  linear 
system.  Consider  its  homogeneous  form  in  which  vld  =  vd  = 
0: 

*(*&)  =  A*d(6tlk+ 1, 8t{+1)x(t{+)  (16) 

where  A*d{5tlk+1, 5t{+1)  is  the  system  matrix  in  (15)  and  its 
entries  depend  on  the  time-varying  Stlk+1  and  Stk+1. 

System  (12)  is  stable  if  p(HAd)  <  1;  however,  this 
criterion  is  not  valid  for  (16)  because  it  is  a  time-varying 
system.  A  sufficient  condition  for  stability  of  (16)  is  quadratic 
stability,  i.e.,  if  we  can  find  a  matrix  P  which  is  time- 
invariant,  symmetric,  and  positive  definite  (denoted  by  P  > 
0)  satisfying 

A*dT(6tlk,  5t{)PA*d(Stlk,  5t{)  -  P  <  0 

for  all  k,  then  (16)  is  quadratically  stable.  However,  this 
condition  is  quite  strong;  in  many  cases  systems  that  are 
stable  cannot  satisfy  the  condition  of  quadratic  stability; 
hence,  it  is  necessary  to  implement  a  weaker  condition, 
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such  as  one  for  poly-quadratic  stability  [14];  to  apply  the 
condition,  the  time- varying  matrix  A^(Stlk,  Stk)  must  be 
converted  into  a  linear  matrix  polytope  form: 

N 

A*d(6t[,6tl)  =  Y/mAi  (17) 

i= 1 
N 

di{k)>  o,  £)&(*}■ -1  (18) 

i=  1 

where  in  right  hand  side  of  (17),  the  time-varying  errors  5tlk 
and  Stk  are  only  contained  in  the  scalar  coefficients  ^(fc), 
leaving  A1 . . .  a  set  of  constant  matrices.  Generally, 
A*d(stlk’stl)  can  hardly  be  converted  into  the  form  like 
(17)  since  Stlk  and  Stk  both  appear  in  A^(Stlk,Stk)  in 
nonlinear  forms.  However,  due  to  the  fact  that  5tlk  and  Stk 
are  very  small,  we  can  use  the  first-order  approximation  of 
A^(Stlk,5tk),  which  yields: 

A*d(5tlk,5t{)  =  HfeA5tiHleA(-T”+Stl^ 

«  Hf(I  +  6tfkA)HieATv(I  +  St[A) 

=  A!k  +  5tkA2  +  StkA'z  +  St^St^A^  (19) 

where  A[  =  Hf HieAT’\  A'2  =  HfHteATvA,  A'z  = 
HfAHieATp,  and  A\  =  HfAHieATvA^  _  _ 

By  introducing  another  upper  bound  Stif  =  Stf  x  Stf , 
which  satisfies  —Stif/ 2  <  StlkStl  <  Stif/2 ,  using  (13),  (14), 
(19)  can  be  rearranged  as 


B.  Optimal  Interaction 

There  are  numerous  objective  functions  that  can  be  se¬ 
lected  to  optimize  the  physical  interaction,  but  it  is  difficult 
to  find  one  which  is  “human-like”.  Despite  the  frequently 
used  functions  (e.g.,  minimum  joint  torque,  minimum  jerk, 
etc.)  that  explain  an  individual’s  behavior,  determining  the 
objective  function  in  pHHI  is  still  a  challenge.  Here  we  use 
the  minimum  interaction  force  as  our  goal  of  optimization. 
This  objective  has  been  adopted  in  many  pHRI  applications 
where  it  is  named  as  “transparency”  [15].  Therefore,  from  the 
robot  follower’s  point  of  view,  the  task  of  optimal  interaction 
is  as  follows: 

At  moment  tk~,  the  robot  collects  all  necessary  infor¬ 
mation  (e.g.,  xi(tk~ ),  xi(tk~ ),  etc.)  to  generate  an  opti¬ 
mal  Xf(tk+ ),  with  which  the  accumulated  interaction  force 

p— 

J  f^1  f2{t)dt  can  be  minimized.  However,  as  the  robot 

tk  , 

cannot  predict  the  exact  tk+1  which  is  up  to  the  human, 
while  f(t)  is  continuous  on  t  and  Stlk+1  is  small,  finally  the 
objective  function  to  be  minimized  is 

ptjF-\-Tp  ~\~TV 

F=  f2(t)dt=  /  xT  (t)cT cx(t)dt 

^ tfk+ 

=  xT(t{+)  (^j\eAt)TcTceAtdt 

=  xT(t{+)Qx(t{+)  (23) 


A*d(stik,st{)  =  ^2mAi 


where 


Sti  A ,  Stif 


4 ,  _  4>  _  2214'  _  "AIL  a' 

Ai  —  2  A'1  2  >1’4’ 


(20) 


(21) 


A-2  — 


tm-  1  &(k)  Uk)  e.(A-) 

A1  +  3 WtA'2,  &(fc)  =  ^  ' — 

3  oti 


which  is  the  same  with  the  problem  given  previously  [10]. 
However,  in  [10],  as  the  robot  could  not  access  x(tk~),  we 
had  to  used  the  gradient  descent  of  F  and  v^.  In  contrast, 
now  we  assume  all  states  in  x(tk~ )  are  accessible,  while  the 
goal  is  to  find  a  Xf(tk+)  that  minimizes  F. 

Function  (23)  only  contains  one  variable  Xf(tk+)  (here¬ 
after  Xf  is  used  for  short).  Let  Qij  be  Q’s  entry  on  row  i  and 
column  j,  Xi  be  the  ith  component  of  x  (hence  X3  =  Xf), 
(23)  can  be  written  as  a  simple  quadratic  function  of  x/ 


A3  —  A1 


A4  =  A1+36tlfAi,£4(k)=  k 


Stk 

F 

=  a2x2f  +  aix f  +  a0 

3  Stf 

d2 

=  Q33 

StkStk+Stu/2 

CLi 

—  2[^31?  ^32?  0?  ^34?  ^35]^  —  Vx 

3  Stif 

Equation  (20)  and  (21)  satisfy  the  required  form  in  (17) 
and  (18),  now  the  internal  stability  can  be  examined  using 
the  condition  proposed  in  [14]:  if  there  exist  four  symmetric 
positive  definite  matrices  S1  . . .  S4  >  0,  and  four  regular 
matrices  . . .  G4,  which  satisfy 

Gi  +  GF-S,  gTaT  .  , 


S, 


(24) 

F  is  minimized  if  Xf  PI  —yxj^a^).  Since  Q  is  assumed  to 
be  time-invariant,  a\  and  a 2  can  be  obtained  off-line,  making 
the  on-line  calculation  of  Xf  quite  simple.  However,  as  kc 
and  dc  in  (9)  are  unknown,  as  well  as  the  errors  Sx  contained 
in  the  measured  x ,  the  calculated  Xf  is  not  the  real  system’s 
optimal  value:  x j  =  — ?7*cc*/(2a2),  the  error  is 


>  0 


xf 


for  alii  =  1, . . . ,  4  and  j  =  1, . . . ,  4,  then  the  system  is  poly- 
quadratically  stable.  The  constraints  of  £1  ...S4  >0,  along 
with  (22)  form  twenty  linear  matrix  inequalities  (LMIs), 
whose  feasibility  can  be  checked  by  a  standard  LMI  solver. 
When  A1 . . .  A4  are  given,  we  can  gradually  find  the  upper 
bounds  of  Sti  and  Stf  by  iteratively  changing  their  values 
and  examining  the  LMIs’  feasibilities. 


_  JL\  T*  _  ZL 


2  a% 


2a2  J 


2a2 


-6x 


~^2a*2  2^2ll®*Uax  +  l2^||^|max-ea:/  (25) 

where  the  absolute  value  of  a  vector  y  is  define  as  \y\  = 

[  I  Vi  1 5  •  •  •  5  \yn  \\T  1  3.nd  |  y  |  max  =  [  1 2/i  |  max  5  •  •  •  •>  |^n|max]"^-  Error 
of  F  is 

6f  =  CL2^xf  —  a2eXf2  =  LP  (26) 
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According  to  (3),  Xf  and  v d  are  related,  which  yields 


Si 

Tf 


TfCf  . 

xf  +  -^Xf 


gx 


g  =  - 


Si 

Tf 


Q 31  Q32  n  Q 34  r/C/  Q35 

5  5  U,  o’ 

a2  a2  a2  5/  a2 


(27) 


Equation  (27)  reveals  that  minimizing  the  interaction  force 
is  equivalent  to  using  state  feedback  with  a  proper  gain  g. 
From  (15)  and  (27)  we  have 


x(tl+i)  =  {Hf  +  Bfg)eA8t^HleA^+<+^x{t{+) 
+{Hf  +  (28) 


With  the  introduced  state  feedback,  stability  of  the  system 
will  be  affected;  therefore,  the  poly-quadratic  stability  should 
be  re-evaluated  by  using  the  LMIs  in  (22). 

IV.  Simulation  and  Experiment 
A.  Simulation 

Parameters  of  simulation  are  as  follows:  for  the  leader 
dancer,  mi  =  70  kg,  zi  =  1.1m;  for  the  follower,  mf  = 
45  kg,  Zf  =  0.9  m,  and  the  nominal  period  of  beat  moments 
is  Tp  =  0.75  s. 

On  poly-quadratic  stability,  two  sets  of  dancers’  upper 
bounds  of  timing  errors  have  been  tested  for  different  com¬ 
binations  of  kc  and  dC9  as  shown  in  Figure.  4.  As  expected, 
smaller  Sti  and  Stf  lead  to  larger  stable  region  of  kc  and  dc. 


On  optimal  interaction,  Q  is  obtained  by  using  kc  = 
60N/m  and  dc  =  25  Ns/m.  Here  we  set  |cc*|max 
0.5.  1.5.0.  1 .5.  0.3]T  and  \Sx\max  =  [0.1, 0.1, 0, 0, 0.05]T; 
the  contours  of  and  \fEp,  which  are  caused  by  the 
differences  between  kc,  dc’ s  nominal  and  real  values,  are 
given  in  Figure.  5.  Notice  that  and  are  non¬ 

zero  even  kc ,  dc  are  the  same  with  their  nominal  values, 
suggesting  that  both  error  bounds  are  somehow  loose. 

The  interactions  between  two  FIPMs  are  simulated,  while 
the  leader  is  supposed  to  have  time-varying  desired  veloc¬ 
ities  vld(tlk).  The  “dance”  starts  from  t[  «  0.75  s,  while 
{vld(tlk)}  =  (1. 1,0.3,  0.1, 1.1,  0.3,  0.1, 1.1,  0.3,  0.1)m/s  for 
k  =  2, . . . ,  10.  Non-optimal  interactions,  in  which  the 
follower  keeps  a  constant,  predefined  vd  =  0.6  m/s,  are 
compared  with  optimal  interactions  in  which  vd  is  obtained 


(a)  Sti  =  0.1s,  Stf  =  0.1s 


10  20  30  40 

dc  (Ns/m) 


(b)  Sti  =  0.08  s,  Stf  =  0.05  s 


Fig.  4.  Contours  of  poly-quadratic  stability  with  respect  to  kc  and  dc\  light- 
colored  area  inside  the  0  border  corresponds  to  poly-quadratically  stable 
combinations  of  kc  and  dc 


Fig.  5.  Contours  of  eXf  and  v/Cf  with  respect  to  kc  and  dc,  whose 
nominal  values  are  chosen  as  kc  =  60N/m  and  dc  =  25  Ns/m 


by  using  (27);  results  are  given  in  Figure.  6;  we  can  see  that 
the  optimal  interaction  with  reduced  force  can  be  achieved 
by  using  (27);  this  method  is  also  insensitive  to  timing  errors 
and  time- varying  kc,  dc,  which  were  not  considered  when  we 
analyzed  the  stability  in  Section  III-A.  Since  poly-quadratic 
stability  is  a  subset  of  stability,  it  is  possible  that  systems 
do  not  satisfy  (22)  can  still  be  stable.  Trajectory  generated 
by  FIPM  and  the  real  human  dancer  have  been  compared  in 

[9]. 

After  introducing  the  state  feedback,  the  stability  of  the 
system  is  re-evaluated,  the  contours  are  shown  in  Figure.  7. 
Compared  with  Figure.  4,  it  can  be  observed  that  proper 
feedback  gains  may  lead  to  the  increased  area  of  stable 
region. 

For  the  case  of  Figure.  6(c),  the  calculated  {vd(tk)}  are 
(0.3,  0.2, 1.1,  0.4,  0.1, 1.2,  0.3,  0.1)m/s  for  k  =  3, . . . ,  10, 
which  are  very  close  to  {vld(tlk)}.  This  matches  the  intuition 
that  in  a  “good”  pHRI,  human’s  and  robot’s  desired  velocities 
should  be  almost  the  same.  However,  when  the  measurement 
error  dx  is  included  in  the  simulation,  the  error  is  amplified, 
causing  {vd(tk)}  seriously  deviate  from  {vld(tlk)},  and  con¬ 
sequently  large  interaction  force,  as  shown  in  Figure.  8. 

Because  the  feedback  gain  g  =  [14.3, 4.8, 0, 0.2, —0.1], 
the  measurement  noise  on  xi  will  be  amplified  by  about 
fourteen  times.  At  the  same  time,  this  error  can  hardly  be 
decreased  as  human’s  pivot  point  can  be  located  anywhere 
inside  the  support  polygon.  Due  to  the  above  facts,  the 
method  given  in  (27)  cannot  be  implemented  in  experiments. 
In  essence,  this  large  amplification  is  the  result  of  FIPM’s 
intrinsic  instability:  very  small  differences  in  initial  condi¬ 
tions  will  increase  exponentially  during  one  step.  In  contrast, 
a  human  dancer  is  empirically  less  sensitive  to  the  initial 
conditions.  Therefore,  in  pHRI  experiment,  if  we  can  find 
another  g  which  can  also  approximate  {vld(tlk)}  while  having 
a  small  gain  on  xi,  we  will  have  an  alternative  solution  of 
optimal  interaction. 

B.  Experiment 

To  evaluate  the  analysis  in  Section  III  and  find  alternative 
feedback  gains  for  optimal  interaction,  a  robot  follower  is 
used  in  pHRI  experiment.  This  robot  is  the  same  mobile 
robot  as  appeared  in  [8],  [10],  except  that  two  FRFs  (laser 
range  finders,  Hokuyo  UBG-04FX-F01)  are  now  installed  to 
detect  human  dancer’s  states,  as  shown  in  Figure.  9. 
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(a)  v d  =  0.6  m/s,  no  timing  error,  (b)  v d  =  0.6  m/s,  with  unknown, 
kc  and  dc  are  known;  F  =  35.1  time-varying  kc  and  dc ,  and  timing 

error;  F  =  48.2 


0  2  4  6 


0  2  4  6 


Time  (s) 


Fig.  8.  Introduced  measurement  error  causes  large  interaction  force,  F  = 
180.0;  |<5a;|max  =  [0.1, 0.1,  0, 0,  0.05]T 


(c)  Optimal  vjj,  no  timing  error,  kc  (d)  Optimal  vj'r  with  unknown, 
and  dc  are  known;  F  =  12.8  time-varying  kc  and  dc,  and  timing 

error;  F  =  14.0 


Fig.  6.  Results  of  simulated  two-LIPM  interaction;  thin  and  thick  curves 
in  velocity  plots  are  the  leader’s  and  the  follower’s  CoM  velocities;  vertical 
solid  and  dashed  lines  are  the  leader’s  and  the  follower’s  beat  moments. 
Timing  errors  and  time- varying  kc  and  dc  are  included  in  (b)  and  (d),  with 
Sti  =  0.1,  Stf  =  0.08,  20  <  kc  <  60,  and  10  <  dc  <  40 


Fig.  7.  Contours  of  poly-< 
introduced 


10  20  30  40 

dc  (Ns/m) 


(b)  Sti  =  0.08  s,  Stf  =  0.05  s 
stability  after  state  feedback  is 


One  LRF  is  used  to  detect  human  leader’s  waist  and  the 
other  is  for  the  ankles.  Because  the  environment  is  quite 
simple  (human’s  waist  and  ankles  are  the  only  clusters  that 
appear  within  the  1.5  m  range),  the  detection  algorithm  is 
straightforward  and  will  not  be  discussed  here.  After  the 
three  clusters  (waist,  left  ankle  and  right  ankle)  are  identified, 
their  mean  values,  which  are  denoted  by  lw  (waist),  //  (left 
ankle)  and  lr  (right  ankle)  are  used  for  later  stages. 

The  information  needed  is  human  leader’s  beat  moments, 
xu  and  xi  (xf  and  Xf  are  robot’s  states,  which  can  be 
obtained  with  ease).  The  beat  moments  can  be  inferred  by 
detecting  feet  landing  using  a  set  of  criteria,  taking  the  left 
foot  for  example,  left  foot  landings  occur  at  moment  t!  if: 

1)  max(t,_0.6<t<t')  \ii(t)\  >  0.5 m/s 

2)  |  h  (t  )  |  <  Uthres 

3)  h(tf)  <  0.5 m 

4)  min(t/_o.3<t<t')  \h(t)\  >  ^thres 

are  all  satisfied.  uthres  is  a  threshold  used  for  adjusting  the 


robot’s  delay  Stk.  In  experiment  uthres  =  0.3  m/s  and  the 
corresponding  delay  is  about  0.05  s 

x\  can  easily  be  acquired  by  using  x\  =  Xf  —  lW9  while 
to  get  xu  additional  procedures  are  needed,  xi  is  defined 
as  the  human  dancer’s  CoM  position  with  respect  to  the 
pivot  position;  hence  firstly  we  need  to  know  which  leg 
is  the  support  leg.  This  is  done  by  using  the  feet  landing 
information,  e.g.,  after  left  foot  landing  and  before  right  foot 
landing,  the  ankle  position  of  the  pivot  leg  is  lpm  lu 

It  should  be  noticed  that  lw  and  lp  are  just  mean  values  of 
clustered  points  on  human’s  surface;  an  unknown  bias  must 
be  considered,  xi  is  actually  x\  —  lp  —  lw  —  /bias-  To  obtain 
/bias  ?  the  human  dancer  stands  still  for  several  seconds  while 
lw  and  lp  are  being  sampled;  assuming  xi  =  0,  /bias  is  then 
calculated  and  averaged  over  time. 

At  first,  the  human  leader’s  independent  moves,  in  which 
the  leader  and  the  robot  follower  are  not  in  physical  contact, 
are  recorded.  Assuming  the  leader’s  desired  velocity  at  tlk+1 
can  be  estimated  by  a  linear  combination  of  xi(tlk)  and 
±i(tlk),  i.e.,  vld(tlk+ 1)  =  gixi(tlk)  +  g2±i(tlk),  by  using  least 
squares  for  data  regression,  we  have  g\  «  3.65  and  g 2  « 
1.90.  Therefore  the  gain  used  in  experiment  is 

g  =  [3.65,1.90,0,0,0]  (29) 

Figure.  10  shows  the  recorded  data  in  the  leader’s  indepen¬ 
dent  moves.  We  can  see  that  the  predicted  vld  (danshed  curve) 
can  well  match  the  real  velocities  at  beat  moments. 

Finally,  three  cases  of  pHRI,  namely  damping  mode  (in 
which  the  robot  acts  as  a  free  mass  with  ground  friction), 
constant  vd  mode  (where  vd  is  set  to  0.6  m/s),  and  variable 
vd  mode  (in  which  vd  is  given  by  (27)  and  (29)),  have 
been  tested  and  compared.  To  generate  repeatable  trials 
for  comparison  (rather  than  restricting  human  to  “fit”  the 
proposed  model),  a  row  of  markers  are  attached  to  the  floor; 
the  human  leader  is  asked  to  land  his  toe  on  the  markers 
during  walking.  To  generate  time- varying  vld,  the  markers 
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Fig.  10.  Human  leader’s  velocity  when  walking  without  physical  contact 
with  the  robot  follower;  the  thick  solid  curve  is  xp  and  the  dashed  curve 
is  the  calculated  vld 


are  not  evenly  spaced;  their  positions  from  the  origin  are 
(0.44,  0.77, 1.05, 1.66,  2.22,  2.54)m.  The  interaction  is  ended 
after  the  sixth  step. 

Results  of  the  three  experiments  are  shown  in  Figure.  11. 
As  expected,  Figure.  11(a)  has  largest  F  since  the  robot  is 
totally  passive.  Figure.  11(b)  performs  well  at  the  beginning 
but  leads  to  large  F  at  the  fourth  step.  In  contrast,  in  Figure. 
11(c),  vd  can  follow  vld  and  hence  effectively  reduce  F;  our 
proposed  approach  is  therefore  supported  by  the  comparison. 
At  the  same  time,  although  the  empirical  gains  in  (29)  work 
well  for  one  subject  (the  author),  as  they  are  the  regression 
result  of  the  many  trials  on  the  same  human  leader,  they 
might  be  unsuitable  for  other  subjects.  This  is  the  limitation 
of  the  empirical-gain  based  method. 

V.  Conclusions  and  Future  Work 

In  this  paper,  we  first  introduced  a  spring-damper  con¬ 
nected,  two-LIPM  model  for  describing  two  human  dancers’ 
body  dynamics  in  sagittal  plane.  Because  timing  errors  of 
two  dancers  are  inevitable,  we  considered  the  effects  of 
timing  errors  and  analyzed  the  poly-quadratic  stability  of 
the  system.  A  state-feedback-based  approach  was  proposed 
to  minimize  the  interaction  force;  because  the  feedback  gain 
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(a)  Damping  mode,  F  «  547 


(b)  Constant  vd  mode  (0.6  m/s), 
F  «  217 
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(c)  Variable  vd  mode,  F  fa  29 


Fig.  11.  Results  of  three  cases  of  pHRI;  thin  and  thick  solid  curves  in 
velocity  plots  are  the  leader’s  and  the  follower’s  CoM  velocities;  dashed 
curve  in  (c)  is  the  calculated  vd ;  the  pHRI  ends  after  the  sixth  step 


obtained  from  quadratic  optimization  can  strongly  amplify 
the  measurement  noise  in  simulation,  an  alternative  set  of 
gains  were  used  and  validated  by  experiments. 

However,  the  gain  obtained  in  experiments  relies  much 
on  empirical  data  rather  than  a  thorough  analysis.  At  the 
same  time,  the  current  model  only  considers  translational 
motions  in  one  plane,  leaving  rotational  motions  (body  spins 
and  turns)  unstudied.  These  issues  will  be  included  in  our 
future  work. 


VI.  Acknowledgment 

This  work  was  supported  by  the  Asian  Office  of  Aerospace 
Research  and  Development  (AOARD),  Air  Force  Office  of 
Scientific  Research  (AFOSR)  under  grant  number  FA2386- 
10-1-4126. 

References 

[1]  N.  Stefanov,  A.  Peer,  and  M.  Buss,  “Online  intention  recognition 
for  computer-assisted  teleoperation,”  Proceedings  of  the  2010  IEEE 
International  Conference  on  Robotics  and  Automation  (ICRA),  pp. 
5334-5339,  2010. 

[2]  V.  Duchaine  and  C.  Gosselin,  “Safe,  stable  and  intuitive  control  for 
physical  human-robot  interaction,”  Proceedings  of  the  2009  IEEE 
International  Conference  on  Robotics  and  Automation  (ICRA),  pp. 
3383-3388,  2009. 

[3]  Z.  Wang,  A.  Peer,  and  M.  Buss,  “An  HMM  approach  to  realistic  haptic 
human-robot  interaction,”  EuroHaptics  conference,  2009  and  Sympo¬ 
sium  on  Haptic  Interfaces  for  Virtual  Environment  and  Teleoperator 
Systems.  World  Haptics  2009.  Third  Joint,  pp.  374-379,  2009. 

[4]  H.  Kazerooni,  “Human/robot  interaction  via  the  transfer  of  power 
and  information  signals,”  IEEE  Transactions  on  System,  Man,  and 
Cypernetics,  vol.  20,  no.  2,  pp.  450-463,  1990. 

[5]  S.  Buerger  and  N.  Hogan,  “Complementary  stability  and  loop  shap¬ 
ing  for  improved  human-robot  interaction,”  IEEE  Transactions  on 
Robotics,  vol.  23,  no.  2,  pp.  232  -244,  2007. 

[6]  S.  Sirouspour,  “Robust  control  design  for  cooperative  teleoperation,” 
Proceedings  of  the  2005  IEEE  International  Conference  on  Robotics 
and  Automation  (ICRA),  pp.  1133-1138,  2005. 

[7]  T.  Takeda,  Y.  Hirata,  and  K.  Kosuge,  “Dance  step  estimation  method 
based  on  HMM  for  dance  partner  robot,”  IEEE  Transactions  on 
Industrial  Electronics,  vol.  54,  no.  2,  pp.  699-706,  2007. 

[8]  - ,  “Dance  partner  robot  cooperative  motion  generation  with  ad¬ 

justable  length  of  dance  step  stride  based  on  physical  interaction,” 
Proceedings  of  the  2007 IEEE/RSJ  International  Conference  on  Intel¬ 
ligent  Robots  and  Systems  (IROS),  pp.  3258-3263,  2007. 

[9]  H.  Wang  and  K.  Kosuge,  “An  inverted  pendulum  model  for  reproduc¬ 
ing  human’s  body  dynamics  in  waltz  and  its  applications  in  a  dance 
partner  robot,”  Proceedings  of  the  2010  IEEE/SICE  International 
Symposium  on  System  Integration,  pp.  182-187,  2010. 

[10]  - ,  “Towards  an  understanding  of  dancers’  coupled  body  dynamics 

for  waltz,”  Proceedings  of  the  2011  IEEE/RSJ  International  Confer¬ 
ence  on  Intelligent  Robots  and  Systems  (IROS),  pp.  2008-2013,  2011. 

[11]  J.  Holldampf,  A.  Peer,  and  M.  Buss,  “Virtual  partner  for  a  haptic  inter¬ 
action  task,”  Human  Centered  Robot  Systems:  Cognition,  Interaction, 
Technology,  vol.  6,  p.  183,  2010. 

[12]  K.  B.  Reed,  J.  Patton,  and  M.  Peshkin,  “Replicating  Human-Human 
Physical  Interaction,”  Proceedings  of  the  2007  IEEE  International 
Conference  on  Robotics  and  Automation  (ICRA),  pp.  3615-3620, 
2007. 

[13]  S.  Kajita  and  K.  Tani,  “Study  of  dynamic  biped  locomotion  on  rugged 
terrain — theory  and  basic  experiment,”  Advanced  Robotics,  1991. 

’ Robots  in  Unstructured  Environments’ ,  91  ICAR.,  Fifth  International 
Conference  on,  pp.  741-746  vol.l,  1991. 

[14]  J.  Daafouz  and  J.  Bernussou,  “Parameter  dependent  lyapunov  func¬ 
tions  for  discrete  time  systems  with  time  varying  parametric  uncertain¬ 
ties,”  Systems  &  Control  Letters,  vol.  43,  no.  5,  pp.  355-359,  2001. 

[15]  N.  Jarrasse,  J.  Paik,  V.  Pasqui,  and  G.  Morel,  “How  can  human  motion 
prediction  increase  transparency?”  in  Proceedings  of  the  2008  IEEE 
International  Conference  on  Robotics  and  Automation  (ICRA),  2008, 
pp.  2134-2139. 


3140 


2011  IEEE/RSJ  International  Conference  on 

Intelligent  Robots  and  Systems 

September  25-30,  2011.  San  Francisco,  CA,  USA 


Towards  an  Understanding  of  Dancers’  Coupled  Body 

Dynamics  for  Waltz 

Hongbo  Wang  and  Kazuhiro  Kosuge 


Abstract — In  this  paper,  we  study  two  dancers’  coupled  body 
dynamics  when  dancing  a  waltz.  A  linear  inverted  pendulum 
(LIPM)  model  for  biped  locomotion  is  utilized  as  each  dancer’s 
dynamic  model,  and  a  balance  controller  for  each  dynamic 
model  is  introduced.  A  pair  of  dancers  are  then  modeled 
as  two  spring-damper-connected  LIPMs  with  their  respective 
controllers.  Assuming  a  perfect  rhythmic  and  synchronized 
motion,  we  analyze  the  stability  of  the  physical  interaction. 
Stable  interaction  with  minimal  interaction  force  is  used  as 
the  criterion  for  optimal  interaction,  which  is  transformed  into 
a  quadratic  programming  problem  and  solved  by  gradient 
descent.  Simulations  and  experiments  show  the  proposed  ap¬ 
proach  for  analysis  of  the  coupled  dynamics  is  reasonable. 

Index  Terms —  Physical  human-robot  interaction,  dance  part¬ 
ner  robot,  linear  inverted  pendulum,  optimal  physical  interac¬ 
tion. 

I.  Introduction 

In  human-involved  physical  interactions,  such  as  pHHI 
and  pHRI  (physical  human-human/robot  interaction),  many 
human  capabilities  and  characteristics  for  haptic  communica¬ 
tion,  body  coordination,  etc.,  are  observed.  Understanding  the 
interactions  helps  designing  a  robot  that  physically  interacts 
with  a  person  in  more  intuitive  ways,  and  many  related 
investigations  have  been  directed.  On  the  one  hand,  because 
the  interactions  involve  at  least  two  independent  entities, 
some  research  concentrates  on  how  intentions  are  conveyed 
and  how  motion  coordination  is  established  [1]— [3] ;  on  the 
other  hand,  since  physical  interaction  is  the  result  of  two  (or 
more)  systems’  coupled  dynamics,  another  category  of  in¬ 
vestigations  focus  on  studying  the  human-involved  dynamics 
[4]— [6] .  We  may  categorize  the  interactions  into  two  levels: 
intention  communication  as  the  higher  level,  and  coupled 
body  dynamics  as  the  lower  level.  In  this  paper,  we  consider 
the  coupled  body  dynamics  during  dancing. 

Generally  we  assume  that  a  human’s  motion  has  repeata¬ 
bility  and  predictability  under  specific  circumstances,  i.e., 
there  is  an  assumed  human  model.  An  ideal  human  model 
would  be  the  exact  model  of  a  human’s  body  dynamics; 
however,  due  to  the  complexity  of  human’s  body  dynamics, 
the  random  factors  contained  in  a  human’s  motion,  and  the 
unknown  control  method  that  human  is  using,  the  exact 
model  is  still  beyond  our  knowledge.  In  practice,  the  human 
model  is  usually  obtained  with  some  assumptions  and  sim¬ 
plifications.  For  instance,  a  linear  mass-spring-damper  model 
could  be  used  to  model  an  operator’s  arm  in  interaction 
with  a  master  [7];  features  of  the  coupled  body  dynamics 
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in  a  specific  task  may  be  implicitly  included  in  an  updated 
probability  density  function  [6],  etc. 

Waltz  is  a  typical  example  of  pHHI.  The  goal  of  this 
research  is  to  replicate  the  pHHI  in  pHRI  by  developing 
a  dance  partner  robot  which  plays  a  role  of  the  female 
dancer.  Since  the  higher  level  interaction,  i.e.,  the  intention 
communication  in  waltz  has  been  studied  in  [8];  our  current 
focus  of  this  paper  is  the  lower  level  interaction  of  the  two 
dancer’s  body  dynamics.  In  one  previous  work  [9],  we  as¬ 
sume  that  a  human  dancer’s  model  contains  one  parameter — 
stride  length.  The  robot  is  able  to  learn  this  parameter  from 
trials  and  use  it  to  scale  the  robot’s  pre-recorded  trajectories. 
To  deal  with  variability  and  randomness  in  human’s  motions, 
the  robot  was  modeled  as  an  inertial  mass,  trying  to  follow 
a  predefined  trajectory  while  being  affected  by  external 
force/torque  and  ground  friction.  This  approach  is  effective 
in  experiments,  but  its  core  idea  relies  on  more  empirical 
knowledge  than  a  quantitative  analysis  of  the  system.  And 
some  important  characteristics  of  the  lower  level  interaction 
may  still  be  hidden.  This  motivates  us  to  model  the  two 
dancers’  body  dynamics,  analyze  the  physical  interaction 
quantitatively,  and  implement  the  acquired  knowledge  in  the 
dance  partner  robot. 

A  simplified  model  of  human’s  dynamics  in  walking  is  the 
linear  inverted  pendulum  (LIPM)  model  [10],  [11].  Since 
waltz  is  different  from  normal  walking  and  there  is  no 
accuracy  requirement  on  the  vertical  motions  of  CoM  (Center 
of  Mass),  the  LIPM  model  is  able  to  reproduce  human’s 
motions  in  some  elementary  waltz  steps  [12].  We  focus  on 
the  elementary  steps  because  they  serve  as  simple  cases  to 
start  with:  elementary  steps  like  closed  changes  involve  no 
spin  or  turn,  the  CoM  motions  can  be  decomposed  into  two 
independent  and  one-dimensional  motions. 

In  Section  II,  we  use  LIPM  as  the  simplified  model  for 
dancer’s  body  dynamics  and  design  a  balancing  controller 
for  the  LIPM.  In  Section  III,  the  two  coupled  dancers  are 
modeled  as  two  connected  LIPMs  and  their  dynamics  are 
analyzed  with  some  assumptions  on  motion  synchronization. 
In  Section  IV,  simulation  and  experiment  results  are  shown. 
Conclusion  is  given  in  Section  V. 

II.  Model  of  A  Single  Dancer  With  Proposed 
Balance  Controller 

A.  The  Simplified  Human  Body  Dynamics  Model 

Consider  a  simplified  human  body  model  in  sagittal  plane 
with  a  massless  leg,  as  shown  in  Fig.  1(a).  By  introduc¬ 
ing  some  constraints,  we  can  obtain  an  inverted  pendulum 
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(LIPM)  model  with  linear  dynamics  and  one  DOF  along  x- 
axis  [10]. 

X  =  ( g/z)x  +  (l/(m^))«ank  (1) 

where  x  is  the  position  of  CoM  with  respect  to  LIPM’s  pivot 
point,  g  is  the  gravity  acceleration,  m  is  the  mass  of  the  body, 
and  r^ank  is  the  torque  input  to  the  ankle. 

B.  Balance  Controllers 

LIPM  is  intrinsically  unstable;  a  balance  controller  is 
necessary.  The  orbital-energy -based  controller  [10]  keeps 
LIPM  on  the  desired  energy  level;  while  another  controller 
[11]  tries  to  have  the  LIPM  reach  desired  position/velocity  at 
a  desired  moment  by  using  optimization.  The  third  controller 
in  [12]  has  LIPM  to  follow  a  series  of  desired  velocities.  In 
the  following  we  will  briefly  introduce  the  above  controllers. 

Let  {tk}  be  a  set  of  moments  when  the  pivot  position  (i.e., 
position  of  the  LIPM’s  pivot  point)  changes.  When  tk  <  t  < 
tk+ 1,  and  if  umk(t)  =  0,  the  orbital  energy  [10]  is  constant 
throughout  the  motion.  This  can  be  written  as: 

E  =  —  {mg)/{2z)x2  +  ( m/2)x 2  (2) 

Assume  that  x  does  not  vary  before  and  after  tk  (this  is 
denoted  by  x+  =  x~ ),  the  controller  can  have  the  LIPM  stay 
at  a  desired  energy  level  by  choosing  a  new  pivot  position. 
However,  since  the  relationship  between  desired  trajectory 
and  orbital  energy  is  implicit,  this  controller  is  inconvenient 
for  trajectory  planning  and  may  cause  problems  like  limping. 

In  waltz,  pivot  positions  are  being  changed  periodically, 
synchronized  with  music  beats.  Assuming  this  period  is  T, 
an  alternative  controller  can  be  derived  by  solving  (1): 

(  x(kT  +  T)\  (C  tS  \  /  x(kT)  \ 

V  x(kT  +  T)  )  \S/t  C  )  \  x(kT)  )  w 

where  r  =  \/  zj  g,  C  =  cosh(T/r),  S  =  sinh(T/r). 

Theoretically,  x(kT ),  which  is  the  CoM’s  relative  position 
from  the  pivot  point,  can  easily  be  calculated  when  desired 
x(kT  +  T)  and  x(kT  +  T)  are  given;  however,  the  square 
matrix  in  (3)  is  ill-conditioned.  One  approach  is  minimizing 
a  defined  error  norm  [11];  another  one  is  to  only  guarantee 
x(kT  +  T): 

x+  =  -( rC/S)x(kT )  +  ( r/S)vd(k  +  1)  (4) 

where  vd(k  +  1)  =  x(kT  +  T )  and  hereafter  we  will  use 
vd(k)  to  denote  the  desired  velocities  at  t  G  {tk}.  The  above 
two  controllers  are  convenient  for  use,  since  the  desired 
position,  velocity,  and  time  are  explicitly  included;  however, 


(a)  Simplified  human  model  (b)  Linear  inverted  pendulum 

Fig.  1.  Simplification  of  body  dynamics 


their  stabilities  are  difficult  to  prove.  In  the  following  we  will 
extend  the  previously  proposed  controllers. 

C.  The  Proposed  Controller 

Assume  that  the  pivot  position  of  an  LIPM  can  change 
instantaneously  without  a  double  support  phase  (where  both 
legs  touch  the  ground),  the  system  can  be  considered  as  a 
hybrid  dynamical  system  with  impulsive  effects  [13].  Let  x 
be  the  CoM  position  with  respect  to  the  pivot  position  and 
x  =  [x,x]T,  we  have 

X  —  ASX  BgUzaiki  t  7^  tk  ✓rx 

x+  =  hk(x~),t  =  tk 

where  x~ ,  x+  are  the  state  vectors  before  and  after  tk.  Our 
goal  is  to  find  a  function  hk  which  can  balance  the  LIPM 
while  keeping  it  following  desired  velocities;  uar±  is  only 
used  for  rejecting  disturbances. 

Given  a  series  of  desired  velocities  {vd(k)}  at  {tk}  and 
define  an  orbital  energy  function  £’(gi,g2)  as 

E0(qi,Q2)  =  ~(mg)/ (2  z)q\  +  (ro/2)gf  (6) 

The  desired  orbital  energy  at  t  =  is 

Ed  =  E0(xd,vd(k))  (7) 

where 

xd  =  -(rC/S)vd(k)  #  (r/S)vd(k  +  1)  (8) 

Equation  (8)  looks  like  (4).  However,  vd(k)  is  used  to 
approximate  x(kT ),  since  Ed  should  be  independent  of 


system’s  states,  . 

In  contrast,  the  energy  corresponding  to  (4)  is 

E*  =  E0(x*,x)  (9) 

where  x*  is  obtained  from  (4). 

Energy  at  t  =  t^  is 

E~  =  E0(x,x)  (10) 

Here  we  define  a  scalar  a ,  with 

a  =  sat{(Ed-E*)/(Ed-E~))  (11) 

where  sat(g)  is  saturation  function:  sat(g)  =  q  if  \q\  <  1, 
sat(g)  =  1  if  q  >  1,  and  sat(g)  =  — 1  if  q  <  —1. 

In  our  proposed  method,  the  desired  energy  at  is 

E+  =  olE~  #  (1  -  a)Ed  (12) 

Therefore,  we  have 

x+  =  sign(xd)^(z/g)x2  -  2z/(mg)E+  (13) 

which  is  the  output  of  hk  in  (5). 

When  t  £  {tk},  u-ank  is  controlled  to  satisfy 

^ank (t)  =  —Kumz  (Eo (x,  x)  -  Ed)  X  (14) 

where  Ku  is  a  positive  gain. 


Since  {vd(k)}  explicitly  defines  the  desired  trajectory,  the 
controller  described  in  (7)— (14)  is  easier  to  use  than  a  pure 
energy  controller.  Now  we  need  examine  its  stability. 


2009 


Choose  a  scalar  function: 


V(x,t)  =  (E(x,t)-Edf>  0  (15) 

When  7  =  7/ c,  we  have 

V(x+,t+)-V(x-^) 

=  (a2-l)(E~  -Ed)2  <0  (16) 

When  t  tk, 

dV/dt  =  —2Kum  (E(x,  7 )  —  7^/)2  x2  <  0  (17) 

Then  the  trajectory  defined  by  E(x,  7)  =  Ed  is  an  asymp¬ 
totically  stable  invariant  set.  Given  constant  Vd(k)  =  Vd,  if 
tk+ i  —  tk  =  T  for  any  k  and  if  Vd(m)  can  be  reached  at 
7m,  then  when  7  >  7m,  the  intersection  of  the  invariant  set 
and  x  =  Vd  is  a  stable  equilibrium  point  of  a  Poincare  return 
map  ( Theorem  7,  [14]). 

The  proposed  controller  can  be  graphically  interpreted  by 
Fig.  2.  When  7  =  7^ ,  the  energy  level  of  E+  is  restricted 
by  2Ed  —  E~  and  E~ ,  which  is  the  area  between  the  two 
solid  lines  in  Fig.  2.  If  E *  is  within  this  area  (as  E*),  the 
controller  is  equivalent  to  (4);  if  E*  lies  outside  (as  E £), 
then  it  can  be  considered  as  a  better  energy  controller  (since 
2 Ed  —  E~  is  closer  to  E%  than  Ed). 

III.  Model  and  Analysis  of  Two  Dancers  With 
Physical  Connection 

A.  Two  Dancers  Having  Physical  Interaction:  Connected 
LIPMs 

In  waltz,  two  dancers  are  physically  connected  by  the 
dance  frame.  We  assume  that  this  system  can  be  represented 
by  a  pair  of  LIPMs  connected  by  a  spring  and  a  damper,  as 
shown  in  Fig.  3.  Let  x\  and  x\  be  the  CoM  positions  of  the 
two  LIPMs,  and  p\  and  p2  be  their  pivot  positions,  all  with 
respect  to  the  global  frame.  Consider  the  case  when  7  =  0, 
Pi  —  P2  =  do  where  —do  is  also  the  natural  length  of  the 
spring.  The  state  vector  x  is  defined  as 

X  =  [xi,Xi,X2,X2,w]T  (18) 

where  x\  =  x\  —  pi,  X2  =  x92  —  p2 ,  and  w  =  p\  —  p2  —  do. 
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Fig.  2.  Illustration  of  the  proposed  controller 


Suppose  u-ank  =  0  for  both  dancers,  The  dynamics  of  the 
connected  LIPM  pair  is 


x  =  Ax ,  7  7^  tk 


(19) 


with 
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where  mi,  m2,  z%  and  Z2  are  masses  and  heights  of  two 
LIPMs,  g  is  the  gravitational  constant,  and  fcc,  dc  are  spring 
and  damping  constants  of  the  connection. 

Two  assumptions  are  made  here: 

1)  For  two  given  LIPMs,  A  is  a  constant  matrix,  i.e.,  mi, 
m2,  zi,  Z2,  kc ,  and  dc  are  all  constant  values. 

2)  Pivot  positions  change  synchronously,  and  7^+i  —  7^  = 
T  for  all  k. 

With  an  additional  assumption  that  E*  in  (9)  is  between  E~ 
and  2Ed~E~,  the  proposed  controller  becomes  equivalent  to 
the  controller  in  (4).  Although  this  assumption  cannot  always 
hold,  later  in  Section  IV  we  can  show  that  this  simplification 
is  reasonable. 

With  the  above  assumptions,  the  system  dynamics  at  7  = 


tk  is 

CC+ 

= 

+  u. 

7  —  tk 

(21) 

AT  is 

(  0 

TlC, 

Si 

0 

0 

0  ^ 

0 

1 

0 

0 

0 

TV  = 

0 

0 

0 

T2C2 

s2 

0 

(22) 

0 

0 

0 

1 

0 

V  1 

TlCl 

Si 

-1 

T2C2 

s2 

where  tt,  72,  Ci,  C2, 
those  in  (3).  And 


Si  and  S2  are  defined  similarly  as 


r  Tl  n  r2  n  rl  r2  iT 

u  =  1^1.0.  -£-vd2,0,-—vdl  +  -£-vd2]  (23) 

01  O  2  o  1  02 

B.  Stability  of  Modeled  Interaction 

Let  x(tk)  denote  the  state  at  the  moment  7  =  7^  and 
x~(tk)  be  the  state  at  7  =  7^.  Assume  that  there  is  a  stable 
periodic  trajectory  for  x  with  period  T,  then  the  following 
conditions  should  be  satisfied: 

1)  *i (tk)  =  ±1  (4+ 1),  x2 (4)  =  4  (4+i) 

2)  (4+i)  -  *1(4)  =  ^(4+1)  -  *2(4)  =  r,  where 
r  is  CoM’s  displacement  from  tk  to  7&+1. 

To  maintain  an  invariant  periodic  trajectory,  the  above  con¬ 
ditions  must  be  satisfied,  which  could  be  rearranged  as 


[x  (4+i)T,r]T  =  M[*(4)T,r]T  (24) 

with  M  is  similar  to  an  i6x6  identity  matrix  except  Mi$  = 

M3,6  =  1. 

From  (19),  we  have 


Fig.  3.  The  dancers’  model:  two  LIPM  connected  by  spring  and  damper 


ifk- i-i )  —  Adx{tk)  (25) 
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where  Ad  =  eAT,  which  is  a  constant  matrix  if  T  is 
constant.  Equation  (24)  and  (25)  yield 

(■ -  m)  [x(4)T,  r]T  =  0  (26) 

where 


If  a  non-zero  x(t)  exists,  [x{tk)T ,  r]T  should  be  in  the  null 
space  of  Ad  —  M.  Since  two  rows  at  the  bottom  of  Ad  —  M 
are  0,  the  non-zero  solutions  [x(tk)T ,  r]T  exist. 

When  t  =  t/c+i,  from  (21)  and  (25) 

*(4+i )  =  Nx  (/A.+i )  +  m  =  NAdx(tk)  +  u  (27) 

If  this  discrete  system  is  asymptotically  stable,  all  the  eigen¬ 
values  of  N Ad  should  be  inside  the  unit  circle.  Because  the 
analytical  form  of  Ad  is  usually  formidable,  the  stability 
is  numerically  determined.  Fig.  4  shows  the  relationship 
between  || X(N Ad)  ||max  and  kc,  dc.  It  can  be  shown  that  even 
m,  2,  and  T  are  all  given,  the  pattern  of  ||A(iVAd)||max  is 
still  quite  complicated. 

If  ||A(7VAd)||max  <  1,  the  system  in  (27)  is  stable,  and 
x(tk)  =  ( I  —  N Ad )-1  rx,  k  — >  oo  (28) 


C.  Optimal  Physical  Interaction 

The  optimal  interaction  is  defined  as  follows:  if  the  sys¬ 
tem  is  stable,  given  the  leader  dancer’s  desired  velocities 
{^(k)},  try  to  find  {vd2{k)}  for  the  follower  to  minimize 
the  interaction  force.  Without  loss  of  generality,  we  assume 
Vdx  (k)  =  Vd19  Vd2  (k)  =  Vd2  and  the  two  LIPMs  have  a  stable 
periodic  motion,  then  our  goal  is 


minimize: 


x 

f 


where  c  =  [kc,  dc,  -kc,  -dc,  kc]T . 

Using  the  analytical  solution  of  x  =  Ax  and  (28),  we 
have 

Fmf  f2(t)dt  =  f  xTccTxdt 

J  o  J  o 

=  x(0)T  ^ J  eATtccTeAtdt 

=  utQu 


(a)  kc  <  5000,  dc  <  300 


(b)  kc  <  200,  dc  <  200 


Fig.  4.  Contour  of  || A(ATAd)||max,  given  mi  =  70kg,  z\  =  1.1m, 
m2  =  45  kg,  Z2  =  0.9  m,  and  T  =  0.75  s 


where 

rT 

Q  =  (I  -  NAd)~T  /  eATtccTeAtdt  ( I  -  NAd)  4 
Jo 

is  a  constant  matrix,  if  m,  z,  T,  kc  and  dc  are  given. 

Our  problem  can  be  reformulated  as: 

minimize:  F  =  uTQu 

subject  to:  Hu  =  b  (29) 

where 

/  1  0  0  0  0  \ 

0  10  0  0 

H  0  0  0  1  0 

\  1  0  -1  0  1  / 

b  =  [{r\/ S\)vd1 , 0, 0, 0]T 


The  problem  described  in  (29)  is  a  typical  quadratic 
programming  problem  and  can  be  easily  solved.  However, 
Q  needs  to  be  numerically  integrated  and  the  calculation  is 
often  time  consuming. 

Optimal  result  can  be  expected  if  the  dance  partner  robot, 
i.e.,  the  follower,  can  get  access  to  all  the  state  variables  and 
parameters  of  the  human  dancer.  However,  this  is  usually 
not  possible;  for  physical  interaction,  only  f(t)  is  available. 
Since  F  is  a  quadratic,  differentiable  function  of  Vd2 ,  if  a 
specific  Vd1  exists,  we  can  use  gradient  descent  to  update 
Vd2  at  the  beginning  of  each  period: 


C1 


(fc) 

=  *4  -  7 


p?(k)  _  p(k- 1) 


Sk) 


Ik- 1) 


(30) 


where  7  is  the  step  size. 


IV.  Simulation  and  Experiment 
A.  Simulation 

For  simulation,  we  use  m\  =  70  kg,  z\  =  1.1m,  m2  = 
45kg,  Z2  =  0.9m,  T  =  0.75s,  and  ^(0)  =  Vd2( 0)  =  0. 

The  three  controllers  discussed  in  Section  II  are  compared 
in  simulations,  let  u^(0)  =  0  and  Vd(k)  =  1  for  all  k  >  0. 
m  =  45  and  2  =  0.8.  We  disturb  £3  by  — T/4  and 
continuously  apply  a  2  N  external  force  on  the  single  LIPM, 
simulation  results  are  shown  in  Fig.  5. 

Due  to  the  external  force,  the  energy  error  (the  distance 
between  current  energy  and  desired  energy  levels)  keeps 
increasing,  except  at  {tk}  when  the  controller  is  supposed  to 
work.  Although  the  energy  controller  reduces  energy  error  to 
0  at  {tk}  (Fig.  5(b)),  the  velocity  is  poorly  controlled  (Fig. 
5(a))  with  the  obvious  problem  of  limping  (i.e.,  FIPM  moves 
faster  when  k  is  even  and  slower  when  k  is  odd). 

The  latter  two  controllers  both  yield  acceptable  results  in 
velocity  control  (Fig.  5(c)  and  Fig.  5(e)).  However,  for  the 
velocity  controller,  its  energy  error  is  not  always  decreasing 
at  {tk},  e.g.,  when  k  =  3  and  k  =  5,  the  energy  errors 
increase  by  0.72  J  and  0.87  J,  which  violates  the  requirement 
in  (16).  In  contrast,  the  proposed  controller  can  satisfy  both 
requirements  on  velocity  and  energy  error  control  (Fig.  5(e) 
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Time  (s) 

(a)  Energy  controller:  velocity 


(c)  Velocity  controller:  velocity 


Time  (s) 

(b)  Energy  controller:  energy  error 


(d)  Velocity  controller:  energy  error 


(e)  Proposed  controller:  velocity  (f)  Proposed  controller:  energy  error 


Fig.  5.  Single  LIPM  simulation  results.  Impulses  denote  moments  of  pivot 
changes  (£*.).  Energy  error  is  defined  as  |E?desired  —  E(t)\ 


and  Fig.  5(f));  the  advantage  of  the  proposed  controller  is 
supported  by  simulation. 

The  A:c—cZc— stability  contours  have  been  shown  in  Fig.  4. 
Here  we  choose  a  stable  combination  as  kc  =  100  and 
dc  =  200.  Set  vdl(k)  =  1  for  all  k  >  0.  Two  interactions 
with  different  vd2(k )  are  simulated  and  shown  in  Fig.  6. 
We  can  see  that  even  Vd1  and  Vd2  are  very  different,  the 
interaction  is  still  stable  since  kc,  dc  have  been  properly 
selected.  However,  the  quality  of  the  physical  interaction 
is  affected  by  Vd2 :  if  it  is  poorly  chosen,  the  interaction 
force  would  be  very  large  (Fig.  6(a)).  In  contrast,  if  Vd2  is 
the  solution  to  the  optimization  problem  given  in  (29),  the 
interaction  force  can  be  minimized,  as  shown  in  Fig.  6(b).  To 
quantitatively  evaluate  the  interaction  forces  throughout  the 
interactions,  we  can  implement  a  value  F,  which  is  defined 
as  F  =  /  ( t)dt / (tend  tstart)?  where  tst art  and  £end  are 

the  start  and  end  moments  of  simulation.  Then  we  have 
F  «  225 N2  in  Fig.  6(a)  and  F  «  ION2  in  Fig.  6(b). 

Simulation  of  the  Vd2  updating  based  on  the  gradient 
descent  method  is  shown  in  Fig.  7.  The  original  Vd2  is  set 
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(a)  vd2  =0.1 


Time  (s) 

(b)  vd2  =  v*2  «  1.0309 


Fig.  6.  Simulated  interaction  of  two  connected  LIPM 


to  0.3  and  7  =  2  x  10-4.  The  simulation  results  suggest 
that  Vd2  is  able  to  be  adjusted  so  as  to  minimize  F  by  using 
(30).  From  Fig.  7(c)  we  can  see  that  a  stays  between  0  and 
0.5,  which  implies  the  controller  has  lead  to  the  velocity 
controller  given  in  (4).  Actually,  interaction  forces  usually 
drive  a  single  LIPM  away  from  its  desired  Ed,  this  makes 
the  assumptions  about  N  in  Section  III-A  reasonable. 

B.  Experiment 

Experiments  have  been  conducted  to  test  our  analysis  and 
the  method  for  human-robot  coordination.  The  robot  used  in 
the  experiment,  shown  in  Fig.  8,  is  the  same  mobile  robot 
as  appeared  in  [9].  The  robot  emulates  LIPM’s  dynamics 
by  implementing  a  virtual  internal  model  in  its  controller. 
Parameters  of  this  virtual  LIPM  are  set  as  m2  =  45  kg  and 
Z2  =  0.9  m.  When  no  external  force  is  applied,  the  velocity 
curve  of  robot  is  shown  in  Fig.  9(a),  which  supports  the 
validity  of  the  proposed  controller  in  Section  II.  Since  the 
virtual  dynamics  of  the  robot  have  no  unmodeled  factors,  the 
robot’s  real  motion  is  only  affected  by  disturbances  and  well 
matches  the  simulation  results  as  given  in  Fig.  5(e). 

To  minimize  the  variability  contained  in  human’s  motions 
and  generate  relatively  repeatable  trials,  a  row  of  markers, 
with  0.4  m  separation,  are  attached  to  the  floor;  the  human 
dancer  is  asked  to  keep  his  toes  landing  on  the  marker  during 
walking,  while  his  motion  is  synchronized  by  audio  cues  with 
0.75s  period.  Parameters  of  the  subject  are  mi  zz  70  kg  and 
z\  ^  1.1m.  The  single  subject’s  velocity  curve  is  obtained 
by  a  motion  capture  system  (VICON  460)  and  given  in  Fig. 
9(b). 

When  Vd2  is  fixed  at  0.3  m/s,  results  of  the  pHRI  exper¬ 
iment  are  shown  in  Fig.  10(a).  We  can  see  that  although 
there  is  a  large  difference  between  vdl  and  vd2 ,  the  human’s 
and  robot’s  motions  are  still  coupled  together;  however,  the 
interaction  force  is  quite  large  (F  «  814  N2)., 

When  Vd2  can  be  adjusted  by  (30),  with  7  =  1  x  10-5, 


(a)  Coupled  velocities  and  interaction  force 


8  °'4 


Time  (s) 

(c)  a 


Fig.  7.  Simulated  vd2  adaptation.  Solid  and  dashed  line  in  (a)  are  leader’s 
and  follower’s  respective  velocities;  solid  and  dashed  line  in  (c)  are  leader’s 
and  follower’s  a  given  in  (11). 
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Fig.  8.  Robot  used  in  experiment 
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Fig.  9.  Velocity  curves  of  human’s  and  robot’s  independent  motions 


results  are  given  in  Fig.  10(b).  The  interaction  force  can  be 
decreased  to  F  «  414  N2,  hence  the  validity  of  gradient 
descent  method  is  supported. 

However,  since  interaction  force  is  the  only  feedback  for 
Vd2  updating  and  usually  affected  by  noises  and  unmodeled 
errors  (e.g.  kc  and  dc  of  human’s  arms  may  keep  changing 
according  to  some  adaptation  rules).  Those  unmodeled  errors 
cause  large  difference  between  simulation  (Fig.  7)  and  exper¬ 
iment  (Fig.  10(b)).  It  can  also  be  observed  that  Vd2  converges 
rather  slowly  and  does  not  converge  to  the  optimal  value 
(which  should  be  around  0.7m/s-0.8m/s). 

V.  Conclusions  and  Future  Work 

In  this  paper,  we  first  proposed  a  balance  controller 
for  a  simplified  human  model,  LIPM.  A  pair  of  dancers 
are  modeled  by  two  spring-damper-connected  LIPMs  with 
balance  controllers.  Assuming  the  perfectly  rhythmic  and 
synchronized  motion,  the  physical  interaction  is  analyzed. 
The  proposed  approach  for  realizing  optimal  interaction  has 
also  been  supported  by  simulation  and  experiment.  However, 
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(a)  pHRI  with  fixed  Vd2 


(b)  pHRI  with  Vd2  being  updated 


Fig.  10.  Results  of  the  coupled  dynamics  of  human  and  robot.  Solid  curves 
in  the  first  row  are  velocities  of  the  robot;  dashed  curves  are  velocities  of 
the  human.  F  «  814  N2  in  (a)  and  F  «  414  N2  in  (b). 


currently  our  analysis  relies  on  the  assumption  that  two 
dancers’  pivot  positions  change  in  synchrony,  while  in  reality 
the  synchronization  error  usually  exists  and  may  significantly 
affect  the  coupled  dynamics.  In  addition,  the  gradient  descent 
method  is  slow  and  sensitive  to  step  size.  If  we  can  have 
more  information  about  the  system,  either  by  using  additional 
sensors  or  by  estimating  Q  in  (29),  more  options  might 
become  available.  Another  limitation  is  that  our  current 
model  does  not  include  the  dynamics  of  spins  and  turns, 
which  are  essential  elements  of  waltz.  The  above  issues  will 
be  addressed  in  our  future  work. 
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An  Inverted  Pendulum  Model  for  Reproducing  Human’s  Body 
Dynamics  in  Waltz  and  Its  Applications  in  a  Dance  Partner  Robot 


Hongbo  Wang  and  Kazuhiro  Kosuge 


Abstract — A  linear  inverted  pendulum  (LIPM)  is  used  to 
model  the  human  dancer’s  body  dynamics  in  closed  changes. 
Several  assumptions  are  made:  a  controller  is  proposed  to 
balance  the  LIPM;  the  dance  frame  is  considered  as  a  spring- 
damper  connection;  the  two  dancers  are  assumed  to  choose 
support  positions  independently.  Motions  generated  by  the 
model  are  compared  with  human’s  real  motions.  Results  of 
comparisons  suggest  the  model  and  the  assumptions  are  effec¬ 
tive  in  reproducing  human  dancers’  body  dynamics  in  waltz. 
Issues  in  implementing  the  model  on  a  dance  partner  robot  are 
discussed. 

I.  INTRODUCTION 

Physical  interactions  between  humans  involve  many  as¬ 
pects,  including  bidirectional  flow  of  force/haptic  signals, 
proactive/reactive  responses  to  the  signals,  and  humans’ 
affected  body  dynamics,  etc.  Due  to  the  complexity  of 
human’s  body  and  random  factors  contained  in  human’s 
motions,  many  principles  and  mechanisms  underlying  phys¬ 
ical  human-human  interaction  (pHHI)  are  still  unknown. 
Since  understandings  of  the  pHHI  may  facilitate  designs  of 
robots  which  can  be  intuitively  controlled  through  physical 
human-robot  interaction  (pHRI),  a  number  of  investigations 
have  been  directed  for  studying  and  reproducing  the  subtle 
features  in  the  interaction  [1]— [5] . 

Waltz  is  a  typical  case  of  pHHI.  When  dancing  together, 
two  dancers  can  communicate  with  each  other  through  phys¬ 
ical  interactions.  At  higher  level,  the  force  or  haptic  signals 
can  be  used  by  the  male  dancer  to  suggest  his  selection  of 
the  next  dance  step,  and  the  female  dancer  uses  the  signals  to 
estimate  the  leader’s  intentions.  At  lower  level,  two  dancers’ 
body  dynamics  are  coupled  together;  both  of  the  dancers’ 
motions  are  affected  by  the  interaction  forces,  resulting  in 
modified  motion  trajectories.  To  understand  mechanisms  of 
pHHI  in  waltz  and  apply  them  to  a  dance  partner  robot, 
we  have  proposed  some  methods  for  realizing  intention 
estimation  [6]  and  cooperative  motion  generation  [7].  A 
dance  partner  robot,  PBDR  (Partner  Ballroom  Dance  Robot) 
has  also  been  developed  in  the  aim  of  creating  a  robot 
who  can  dance  with  human  through  physical  interaction  in 
human-like  ways. 

Usually,  researchers  assume  that  repeatability  and  pre¬ 
dictability  exist  in  human’s  behaviors,  in  another  word,  there 
exists  a  human  model  (which  could  be  deterministic  or 
probabilistic,  explicit  or  implicit),  through  which  the  status  of 
interaction  can  be  estimated.  Aside  from  the  human  model, 
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since  interaction  involves  at  least  two  entities,  it  is  also 
necessary  to  implement  a  robot  model  which  defines  the 
robot’s  responses  to  the  force/haptic  signals. 

In  the  case  of  the  dance  partner  robot,  the  human  dancer 
and  the  robot  were  also  modeled  at  higher  level  (step 
intention  suggestion  and  estimation)  and  lower  level  (coupled 
body  dynamics): 

1)  At  the  intention  suggestion/estimation  level,  the  human 
dancer  is  assumed  to  be  capable  of  producing  different 
time-force/torque  patterns  for  different  selected  steps. 
On  the  robot  side,  several  hidden  Markov  models 
(HMM)  were  implemented  to  select  a  step  with  max¬ 
imum  likelihood  [6]. 

2)  At  the  body  dynamics  level,  the  human  dancer  model 
contains  one  parameter — stride  length.  The  robot  is 
able  to  learn  this  parameter  from  trials  and  use  it 
to  scale  the  robot’s  pre-recorded  trajectories.  To  de¬ 
fine  its  responses  to  force/torque  inputs,  the  robot 
was  modeled  as  an  inertial  mass  affected  by  external 
force/torque  and  ground  friction  [7]. 

Although  the  above  models  have  been  proved  to  be  ef¬ 
fective  as  successful  cooperated  dancing  can  be  generated  in 
experiments,  the  simple  inertial  mass  model  has  hidden  some 
important  features  in  the  physical  interaction.  When  the  robot 
acts  like  a  mass  on  the  ground,  which  is  quite  different  from 
the  behavior  of  a  real  female  dancer,  the  resultant  pHRI  will 
be  deviated  from  real  pHHI.  Therefore,  to  make  the  human- 
robot  cooperated  dance  more  life-like  ,  it  is  necessary  to  have 
the  robot  behave  like  a  human  dancer,  i.e.,  the  robot  should 
contain  a  sufficiently  precise  model  of  a  female  dancer’s 
body  dynamics. 

Rather  than  a  free  mass  moving  on  flat  ground,  human’s 
body  dynamics  in  walking  is  often  modeled  as  an  inverted 
pendulum.  The  linear  inverted  pendulum  mode  (LIPM)  [8] 
and  its  3-D  extension  (3D-LIPM)  [9]  proposed  by  Kajita  et 
al.  are  widely  used  for  biped  gait  planning.  The  LIPM  is 
considered  as  simplified  model  since  it  assumes  the  biped 
as  an  inverted  pendulum  with  massless  legs,  while  applying 
some  restrictions  on  its  center-of-mass  (CoM)  trajectories, 
upper  body  rotations  and  ankle  input  torques. 

It  is  certain  that  those  simplifications  and  restrictions  will 
introduce  some  errors,  which  make  LIPM  less  accurate  than 
a  more  sophisticated,  whole  body  model  (e.g.,  a  34-DOF 
humanoid  model).  However,  LIPM  is  an  affordable  model 
for  our  analysis,  especially  when  two  coupled  systems  are 
involved.  The  model’s  simplicity  ensures  that  we  can  find 
some  qualitative  and  (approximate)  quantitative  rules  of  the 
interaction,  and  make  use  of  these  rules  to  control  the  dance 
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partner  robot.  Therefore,  in  this  paper,  we  will  model  a  waltz 
dancer  as  LIPM  and  analyze  his/her  behavior  in  single  and 
paired  dance.  We  will  also  discuss  the  model’s  accuracy, 
as  well  as  issues  in  implementing  the  model  on  the  current 
robot. 

In  Section  II,  a  single  female  dancer  is  modeled  as  an 
LIPM,  comparisons  between  real  motion  data  and  model- 
predicted  data  are  directed  to  evaluate  the  model’s  goodness. 
In  Section  III,  the  two  coupled  dancers  are  modeled  as 
two  connected  LIPM,  dynamics  of  the  coupled  systems 
are  analyzed  and  comparisons  with  the  experiment  data  are 
made.  In  Section  IV,  issues  in  using  LIPM  in  a  dancer  robot 
are  briefly  discussed.  Conclusion  is  given  in  Section  V. 

II.  MODELING  A  SINGLE  DANCER  WITH  LIPM 

A.  The  Linear  Inverted  Pendulum  Mode 

Consider  a  simplified  human  body  model  with  a  massless 
leg  in  sagittal  plane,  as  shown  in  Fig.  1(a).  The  origin  of  the 
coordinate  frame  is  at  the  supporting  point.  By  applying  the 
following  constraints: 

1)  The  CoM  of  the  whole  body  moves  along  a  straight 
line  defined  by  %  =  kx  +  h,  h  0; 

2)  The  angular  velocity  of  the  upper  body  is  a  constant; 
we  can  then  consider  the  system  as  an  inverted  pendulum 
with  a  massless,  extendable  leg  and  a  point  mass,  as  shown 
in  Fig.  1(b).  Since  two  constraints  exist  in  the  3-DOF  system, 
the  resultant  system  has  one  DOF,  with  linear  dynamics 
defined  by  the  following  equation  [8] 

••5.1 

x  =  —x  H - -u  (1) 

h  mh 

where  g  is  the  gravity  acceleration,  m  is  the  mass  of  the 
body,  and  u  is  the  torque  input  on  the  ankle. 

When  the  supporting  point  is  not  at  the  origin,  (1)  can  be 
rewritten  as 

9  9 

X  =  hX~  hP  2 

where  p  is  the  position  of  the  supporting  point.  Notice  that 
the  term  u  in  (1)  does  not  appear  in  (2),  because  changes 
of  u  can  be  substituted  by  changes  of  p\  this  simplification 
is  reasonable,  as  for  real  human  walking,  we  can  consider 
the  changes  of  ankle  torque  as  the  result  of  CoP  (center 
of  pressure)  changes,  p  is  used  as  the  input  to  stabilize  the 
system. 

Although  LIPM  has  been  successfully  implemented  by 
many  biped  walking  systems,  when  to  model  human  dancers’ 
body  dynamics  in  waltz,  some  difficulties  exist: 


far  -r 


(a)  Simplified  human  body  model  (b)  Linear  inverted  pendulum 
Fig.  1.  Simplification  of  body  dynamics 


1)  Human’s  motion  contains  large  variability,  as  human 
body  has  too  many  DOFs  and  can  be  affected  by  too 
many  random  factors.  This  is  a  common  problem  that 
all  assumed  human  models  are  facing. 

2)  LIPM  is  a  largely  simplified  model,  it  might  be  unable 
to  include  many  crucial  features  in  human  dancers’ 
body  dynamics. 

3)  LIPM  is  intrinsically  unstable,  hence  we  need  assume 
a  controller  to  stabilize  LIPM;  however,  the  difference 
between  the  assumed  controller  and  human’s  real  con¬ 
troller  would  introduce  additional  errors. 

In  the  following,  after  modeling  waltz  sequence  with  LIPM 
and  comparing  the  simulation  and  the  experiment  results,  we 
will  examine  and  discuss  the  effects  of  the  above  difficulties. 

B.  Modeling  The  Dancing  Sequence 

Closed  changes  are  the  elementary  steps  in  waltz  as 
no  rotation  is  involved;  closed  changes  can  be  analyzed 
separately  in  sagittal  plane  and  frontal  plane,  while  in  this 
paper  we  concentrate  on  the  dynamics  in  sagittal  plane.  The 
step  diagram  of  left  closed  change  (CCL)  is  given  in  Fig.  2, 
in  which  the  male  dancer  initiates  the  dance  with  his  left 
foot.  Similarly,  right  closed  change  (CCR)  is  the  mirrored 
moves  of  CCL,  as  in  CCR  the  male  dancer  begins  with  his 
right  foot. 

In  this  section  we  are  to  analyze  a  single  dancer’s  body 
dynamics  in  sagittal  plane.  Consider  a  single  dancer’s  moves 
in  CCL/CCR.  If  we  approximate  those  moves  with  the  LIPM 
model,  a  sequence  of  motions  of  an  inverted  pendulum  can  be 
obtained,  as  illustrated  in  Fig.  3.  The  numbers  (1,2,3)  indicate 
moments  of  supporting  point  changes.  In  Fig.  3,  initiaion-1, 
1-2,  and  2-3  respectively  correspond  to  Fig.  2(b),  Fig.  2(c), 
and  Fig.  2(d).  p\  and  p2  are  the  landing  locations  of  Fig.  2(b) 
and  Fig.  2(c).  In  order  to  use  the  LIPM  model,  we  assume  the 
CoM  follows  a  series  of  straight  segments,  which  in  Fig.  3 
are  dashed  lines  with  arrowheads. 

C.  The  Assumed  Controller 

As  mentioned  in  Section  II-A,  an  assumed  controller 
which  guides  and  balances  the  LIPM  is  essential  for  model¬ 
ing  the  dance  sequence.  One  straightforward  method  is  firstly 


(a)  Initial  position  (b)  First  move 


(c)  Second  move  (d)  Third  move 


Fig.  2.  Left  closed  change  (CCL) 
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Fig.  3.  Approximate  motion  sequence  of  CCL 


solving  the  homogeneous  form  of  (2)  and  using  its  solution  to 
control  p.  Assuming  that  when  to  <  t  <  t\  support  changes 
do  not  happen,  i.e.,  p(t)  is  a  constant  for  to  <t  <t\.  Then 
we  have  a  homogeneous  differential  equation 

x'  =  ( g/h)x '  (3) 

where  x'  =  x  —  p  is  the  CoM’s  relative  position  with  the 
supporting  point.  And  for  to  <  t  <  t\,  x'  =  x,  x'  =  x. 
The  analytical  solutions  for  this  equation  are  a  set  of  linear 
combinations  of  et/r  and  e_t/r,  where  r  =  yjh/g. 

In  waltz,  dancers’  support  changes  are  synchronized  with 
music  beats.  If  we  consider  the  constant  beat  period  (denoted 
by  T)  between  two  support  changes,  the  analytical  solutions 
to  (3)  can  be  written  as 

x'(kT  +  T)  =  Cx'{kT)  +  rSx'(kT) 

x\kT  +  T)  =  (S/r)xf  (kT)  +  Cx'(kT)  (4) 

where  r  =  y/ h/g ,  C  =  cosh(T/r),  S  =  sinh(T/r). 

From  (4),  we  have 

x\kT)  =  ~{rC/S)x\kT )  +  ( r/S)x\kT  +  T)  (5) 

The  new  support  position  p(kT)  =  x(kT)  —  x'(kT ),  and  in 
single  support  phase  x(kT)  =  x'(kT ),  hence 

p(kT)  =  x(kT)  +  (rC / S)x(kT)  -  (r/S)x{kT  +  T)  (6) 

Equation  (6)  is  the  proposed  controller  while  x(kT  +  T)  is 
a  reference  velocity  we  want  to  achieve. 

D.  Experiment 

To  evaluate  the  effectiveness  of  the  LIPM  in  modeling 
a  single  dancer’s  body  dynamics,  a  professional  female 
dancer’s  real  motions  were  used  for  comparisons.  CCL  (with 
period  T  =  0.75  s)  was  performed  three  times  by  the  same 
female  dancer  and  her  body  motions  were  measured  and 
recorded  by  a  motion  capture  system  (VICON  460)  at  the  rate 
of  120  frames  per  second.  Since  it  is  difficult  to  distinguish 
the  moment  of  initiation,  the  moments  of  peak  velocities  in 
the  three  trials  were  used  to  align  the  data  along  the  time 
axis,  as  shown  in  Fig.  5(a). 

To  approximate  the  human  dancer’s  motions,  the  LIPM 
model  needs  several  parameters,  which  are  listed  in  Table  I. 
The  first  set  of  parameters  includes  m,  htl,  and  ht2  (htl 
and  ht2  are  respectively  the  heights  of  CoM  at  fall  and 
rise  positions  in  waltz),  which  defines  the  dancer’s  physical 
conditions. 

An  important  point  to  be  noticed  is  that  obtaining  accurate 
CoM  locations  from  motion  capture  data  is  itself  a  challenge 


[10],  [11].  For  simplicity,  here  we  choose  a  point  on  the 
pelvis  as  the  approximate  CoM  location.  Hence  htl  and  ht2 
in  Table  I  are  two  estimations.  Although  causing  inaccuracy, 
the  errors  in  CoM  estimation  are  partly  compensated  by  the 
following  facts: 

1)  In  waltz,  the  human’s  upper  body  keeps  a  fixed  con¬ 
figuration  while  the  lower  body  movements  are  fairly 
moderate;  these  avoid  the  CoM  to  be  deviated  too 
much  from  the  pelvis. 

2)  On  the  model  side,  due  to  the  existence  of  the  con¬ 
troller  (Section  II-C),  the  resultant  movement  is  not 
sensitive  to  htl  or  ht2.  An  example  is  given  in  Fig.  4; 
the  variations  in  htl  and  ht2  does  not  largely  affect  the 
result. 

Another  set  of  parameters  includes  x(T ),  x(2 T)  and 
x(3 T),  which  are  velocities  measured  from  motion  capture 
data  (as  shown  in  Fig.  5(a))  and  represent  some  features  of 
the  female  dancer’s  motions.  x(0)  is  assumed  to  be  0. 

Results  of  the  comparisons  are  given  in  Fig.  5.  The  “+” 
markers  represent  motions  generated  by  the  LIPM  model. 
Aside  from  CoM  positions  and  velocities,  another  set  of 
criteria  is  the  comparisons  of  p(t).  LIPM  gives  the  results  as 
p(T)  =  0.77  m  and  p(2T)  =  0.80  m,  while  the  experiment 
results  are  p(T)  =  0.75  m  and  p(2T)  =  0.90  m.  The  error 
at  p(2T)  is  not  negligible;  however,  considering  the  fact  that 
the  human  dancer’s  supporting  point  (or  CoP)  can  be  located 
anywhere  within  the  support  polygon,  the  above  error  is 
acceptable.  In  addition,  it  should  be  noticed  that  only  x(T) 
and  x(2T)  were  given  to  the  LIPM  model,  other  data  (x  at 
t  kT,  x,  and  p)  are  all  generated  by  the  model. 

According  to  the  results  of  comparisons,  the  LIPM  model 
approximates  the  female  dancer’s  real  motions  quite  well. 
Another  phenomenon  is  that  the  large  variability  expected 
in  human’s  motions  was  not  observed.  Here  we  can  give 
a  tentative  explanation  to  these  results:  because  waltz  has 


Fig.  4.  CoM  positions,  velocities  and  support  positions  generated  by  LIPM 
with  four  sets  of  height  parameters.  First  set:  htx  =  0.9,  ht2  =  1.0;  second 
set:  ht1  =  1.0,  ht2  =  0.9;  third  set:  ht1  =  0.8,  ht2  =  1.1;  fourth  set: 
htl  =  1.0,  ht2  =  1.0. 

TABLE  I 

Parameters  For  LIPM  and  Its  Controller 


m 

«  45  kg 

ht i 

«  0.9  m(T  <  t  <  2T) 

ht2 

«  1.0  m(0  <  t  <  T,  2T  <  t  <  3T) 

x(0) 

0 

x(T) 

«  1.27  m-s-1 

x(2  T) 

^  0.16  m-s i 

x(3  T) 

«  0.10  m-s— 1 
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(a)  CoM  velocities  (b)  CoM  displacements 


Fig.  5.  Comparison  of  theoretical  and  trial  data 


applied  many  constraints  to  human’s  motions  (e.g.,  constant 
walking  period,  CoM  trajectories  regulated  by  rise  and  fall, 
and  straight  upper  body,  etc.),  it  is  possible  that  these 
constraints  might  reinforce  the  repeatability  in  different  trials 
and  make  the  human  body  behave  like  an  inverted  pendu¬ 
lum.  Though  the  underlying  reasons  are  still  not  clear,  our 
proposed  approach,  which  uses  a  simple  inverted  pendulum 
to  model  human’s  body  dynamics  in  waltz,  is  supported  by 
the  comparisons. 


III.  MODELING  THE  PHYSICAL  INTERACTION 

A.  Modeling  The  Interaction  Between  Two  LIPM 

Waltz  involves  physical  interactions  between  two  dancers, 
whose  body  dynamics  can  be  modeled  as  two  LIPM  con¬ 
nected  by  a  dance  frame,  as  shown  in  Fig.  6.  The  equations 
describing  the  dynamics  of  the  two-LIPM  system  are 

X\  =  -  —  (7) 

hi  m  i 

X2  =  J-(X2-P2)  +  I—  (8) 

h2  m2 

where  /  is  the  compression  force  between  the  two  dancers. 

If  we  approximately  consider  the  dance  frame  as  a  rigid 
connection  as  in  Fig.  6(a),  then  we  have  x%  =  x2  —  d ,  x\  = 
±2 ,  and  X\  =  ,t2.  By  defining  the  following  error  of  the 
female  dancer’s  supporting  location  as  e  =  p2  —  Pi  ~  d,  and 
letting  x  =  x\  and  p  =  pi,  the  dynamics  of  the  two-LIPM 
system  are 


Mx 

f 


Kx  —  Kp  —  fc2e 


mim2 

M 


%  h2)[X  P)  +  h26 , 


(9) 

(10) 


where  M  =  m\  +  m2,  k\  =  mig/hi,  /c2  =  m2g//i2,  and 
K  =  k%  +  fc2.  Some  rough  characteristics  of  the  interaction 
can  directly  be  obtained  from  (9)  and  (10):  according  to  (9), 


(a)  Rigid  connection  (b)  Spring-damper  connection 

Fig.  6.  Dancers’  model  as  two  inverted  pendulums 


if  the  female  dancer  follows  perfectly,  i.e.,  e(t)  =  0  for  all  t, 
then  the  two-LIPM  system  will  behave  like  a  single  LIPM, 
with  time  constant  y/ M/K .  According  to  (10),  as  long  as 
h\  ^  /i2,  even  e(t)  =  0  for  all  t,  f(t)  is  still  not  always 
0;  on  the  other  hand,  (10)  also  suggests  the  possibility  of 
eliminating  f(t )  by  controlling  e(t). 

Alternatively,  we  can  also  assume  that  the  two  dancers 
are  connected  by  spring  and  damper  as  shown  in  Fig.  6(b). 
This  spring-damper  assumption  is  more  accurate  than  the 
rigid  one,  as  distance  changes  between  the  two  dancers  were 
observed  in  experiments.  According  to  this  assumption,  the 
interaction  force  /  is 

/  =  -kc(x2  -xi  -  dk)  -  Dc(x2  -±i)  (11) 

where  kc  is  the  spring  constant,  Dc  is  the  damping  ratio, 
and  dk  is  the  natural  length  of  the  assumed  spring.  The  two- 
LIPM  system’s  behavior  is  hence  determined  by  (11)  along 
with  (7)  and  (8). 

B.  Analyzing  The  Interaction 

Interaction  between  two  dancers’  body  dynamics  is  one 
of  the  crucial  features  we  want  to  investigate  and  reproduce 
with  robot.  Because  the  interaction  is  influenced  by  too 
many  physical  and  non-physical  factors  (e.g.,  empirically, 
merely  the  presence  of  a  partner  would  influence  the  dancer’s 
expectations  about  the  future  moves),  it  is  very  difficult  to 
reveal  the  mechanisms  in  the  interaction.  However,  with  the 
LIPM  model  and  some  additional  assumptions,  it  is  possible 
to  analyze  the  interaction  with  the  approximate  model. 

From  the  female  dancer’s  point  of  view,  two  assumptions 
of  the  interaction  can  be  made.  The  first  one  assumes  that 
the  female  dancer  follows  the  male  dancer  perfectly  (i.e., 
P2  =  Pi  +  d  where  d  is  constant),  while  the  second  one 
assumes  the  female  dancer  to  be  completely  independent 
(i.e.,  P2  is  determined  only  by  and  (6),  as  if  she  is  dancing 
alone).  Then  the  above  two  assumptions  are  examined  both 
with  the  rigid-connection  model  (Fig.  6(a))  and  the  spring- 
damper-connection  model  (Fig.  6(b)). 

Simulation  results  based  on  the  second  assumption,  in 
which  the  female  dancer  chooses  p2  independently,  are 
shown  in  Fig.  7.  The  two  dancers  have  different  parameters 
(mi  =  70  kg,  hi  =  1.2  m,  m2  =  50  kg,  h2  =  0.8  m)  but 
are  given  the  same  desired  velocities  at  t  =  kT ,  k  =  1,  2,  3. 
When  kc  =  Dc  =  0,  the  dancers  can  be  considered  as  two 
separated  LIPM  (Fig.  7(a)).  In  contrast,  when  kClDc  ^  0, 


(a)  kc  =  Dc  =  0  (b)  kc  =  Dc  =  1000 

Fig.  7.  Simulation  results:  independent  female  dancer 
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a  physical  connection  is  established  and  dynamics  of  the 
two  dancers  are  coupled  in  spite  that  their  controllers  are 
independent.  The  coupled  velocities  under  the  condition 
kc  =  Dc  =  1000  (which  is  nearly  a  rigid  connection)  are 
shown  in  Fig.  7(b). 

If  the  female  dancer’s  support  position  completely  follows 
Pi  +d  (i.e.,  e(t)  =  0  for  all  t),  the  dance  cannot  be  continued 
if  the  connection  is  too  weak  (e.g.,  kc  =  Dc  =  140),  since 
she  will  rapidly  loose  balance  (Fig.  8(a)).  If  the  connection  is 
strong  (or  rigid),  the  system  can  then  be  balanced  by  the  male 
dancer’s  controller;  however,  the  desired  velocities  cannot  be 
reached,  because  the  male  dancer’s  controller  does  not  take 
the  female  dancer’s  body  dynamics  into  account  (Fig.  8(b)). 

Based  on  the  above  analysis,  it  might  be  reasonable  to 
assume  the  two  dancers’  support  positions  are  independently 
controlled  while  their  body  dynamics  are  coupled  through 
the  physical  connection — the  dance  frame. 

C.  Experiment 

A  dance  sequence  which  consists  of  a  CCR  and  a  CCL  was 
performed  by  two  dancers.  Two  LIPM  with  spring-damper 
connection  are  used  as  the  system  model.  The  two  LIPM 
are  assumed  to  have  independent  controllers.  Because  it  is 
relatively  difficult  to  determine  the  desired  velocities  for  the 
controllers,  the  two  dancers’  real  velocities  at  t  —  kT  were 
used  as  approximations.  Results  of  comparisons  are  given  in 
Fig.  9. 

According  to  Fig.  9(a)  and  Fig.  9(b),  the  assumed  two- 
LIPM  system  and  their  independent  controllers  are  able 
to  model  the  real  dancers’  dynamics.  The  male  dancer’s 
simulated  and  real  support  positions  are  compared  in  Fig. 
9(c).  The  two  dashed  lines  are  toe  and  heel  locations  of  the 
male  dancer;  the  solid  line  is  the  model’s  simulated  support 
position.  If  we  consider  the  dashed  lines  as  upper  and  lower 
bounds  of  a  support  polygon,  then  the  solid  line  should  be 
located  inside  or  on  the  edge  of  the  polygon. 

IV.  DISCUSSION 

A.  Issues  in  Implementing  LIPM  on  a  Mobile  Robot 

Instead  of  focusing  on  kinesiology,  the  final  goal  of 
our  study  is  developing  a  robot  that  can  dance  waltz  with 
human  by  emulating  a  female  dancer’s  behaviors.  Because 
the  previous  model  cannot  represent  dancers’  body  dynamics 
with  sufficient  closeness,  while  the  LIPM  and  the  assumed 


Time  (s)  Time  (s) 

(a)  kc  =  Dc  =  140  (b)  kc  =  Dc  =  2000 

Fig.  8.  Simulation  results:  e(t)  =  0  for  all  t 


Time  (s) 

(c)  support  position  of  the  male  dancer 
Fig.  9.  Comparisons  of  simulation  and  experiment  data 


controller  gave  good  approximations,  it  would  be  better  to 
have  the  robot  to  adopt  the  LIPM  as  its  dynamics  model. 

With  a  motor  actuated,  omni-directional  mobile  base  and 
a  force/torque  sensor  in  the  waist,  the  developed  prototype, 
PBDR,  is  able  to  emulate  an  LIPM’s  dynamics  (given  in  (8)), 
as  long  as  the  speed  and  the  acceleration  do  not  exceed  the 
motors’  limits.  The  controller  described  in  (6)  can  be  used 
to  direct  and  balance  the  robot  at  low  level,  while  another 
high  level  controller  is  also  needed  to  feed  the  low  level 
controller  with  desired  velocities,  which  could  be  obtained 
from  experiments  or  determined  by  other  rules. 

In  essence,  adopting  the  LIPM  model  means  we  are 
turning  an  intrinsically  stable  mobile  robot  into  an  unstable 
inverted  pendulum,  then  applying  an  extra  controller  to  re¬ 
stabilize  the  system.  It  may  look  strange  and  redundant  of 
doing  this,  while  additional  dangers  are  introduced  due  to  the 
new  system’s  instability;  however,  as  our  goal  is  the  life-like 
pHRI,  there  are  many  advantages  in  doing  this: 

1)  The  robot  can  react  to  the  interaction  force  in  a  more 
human-like  way. 

2)  Stride  length  adjustments  are  more  dynamic,  i.e.,  it  is 
not  a  constant  parameter  learned  in  trials,  but  a  com¬ 
promise  between  dance  trajectories  and  body  balance. 

3)  During  the  transition  time  between  two  dance  steps, 
the  robot  is  in  an  unstable  equilibrium  state;  the  motion 
direction  can  easily  be  changed  by  a  slight  force.  This 
fact  has  the  potential  to  be  utilized  to  estimate  the  male 
dancer’s  intentions. 

The  value  of  the  above  advantages  to  the  study  of  pHRI 
makes  it  worthy  to  implement  the  LIPM  model  on  the  dance 
partner  robot. 

B.  LIPM  versus  Other  Models/Methods 

There  are  many  alternatives  to  LIPM:  two  typical  exam¬ 
ples  are  human-like  model  and  curve  fitting.  The  former 
one  is  supposed  to  be  more  precise  than  LIPM,  while  the 
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latter  one  is  supposed  to  be  equivalent  to  LIPM  in  generating 
dancing  trajectories.  Nevertheless,  LIPM  is  still  selected  over 
the  other  two  alternatives;  the  reasons  are  explained  below. 

Although  a  model  with  fewer  simplifications  is  generally 
more  precise  than  LIPM  in  reproducing  human-like  motions, 
it  is  consequently  more  complicated.  The  complexity  of  the 
model  implies  many  difficulties  in  analysis  and  a  result  which 
may  be  hard  to  simplify  and  interpret.  In  addition,  a  precise 
and  complicated  model  usually  requires  more  parameters; 
if  those  parameters  can  not  be  properly  defined  to  reflect 
human’s  real  motions,  the  model’s  fidelity  would  still  be 
decreased.  Due  to  the  above  facts,  selecting  the  model  is 
a  compromise  between  accuracy  and  practicality. 

On  one  hand,  limited  knowledge  of  human’s  dynamics 
and  control  in  waltz,  along  with  the  complexity  of  analysis, 
restrict  us  from  implementing  an  over-complicated  model. 
On  the  other  hand,  as  a  mobile  robot,  PBDR’s  vertical  motion 
is  independent  of  its  horizontal  motion,  while  LIPM  can 
generate  sufficiently  accurate  motion  in  horizontal  plane  (and 
less  accurate  in  vertical  plane);  therefore,  precise  motions  can 
be  yielded  by  using  LIPM  for  horizontal  motion  and  using 
recorded  trajectories  for  vertical  motion.  For  these  reasons 
we  choose  LIPM  as  the  compromise  point. 

Another  comparison  is  between  LIPM  and  curve  fitting 
methods.  Similar  dancing  trajectories,  e.g.,  Fig.  5,  Fig.  9(a), 
and  Fig.  9(b),  can  simply  be  produced  by  curve  fitting. 
Indeed,  if  the  robot  is  supposed  to  dance  on  its  own,  curve 
fitting  is  more  straightforward  than  LIPM.  However,  the 
robot  is  expected  to  physically  interact  with  human  dancer; 
curve  fitting  can  be  used  to  yield  a  trajectory,  but  the  body 
dynamics  model  would  still  be  needed  to  respond  to  external 
forces/torques  (e.g.,  the  term  /  in  (7)  and  (8))  in  a  human¬ 
like  way.  Compared  with  curve  fitting,  LIPM  not  only  fits 
trajectory  but  also  serves  as  a  dynamics  model.  The  purpose 
of  using  LIPM  is  to  facilitate  investigating  the  pHRI  in  waltz, 
which  is  difficult  to  be  addressed  by  curve  fitting  methods. 

V.  CONCLUSION  AND  FUTURE  WORK 

Human  and  robot  models  are  two  crucial  parts  in  pHRI; 
selections  of  those  models  can  largely  affect  pHRI’s  qual¬ 
ities.  Because  the  robot  model  adopted  by  a  dance  partner 
robot  (PBDR)  was  not  sufficiently  accurate  in  representing 
human’s  body  dynamics  in  waltz,  a  linear  inverted  pendulum 
(LIPM)  model,  which  has  been  widely  used  in  biped  gait 
generation,  is  implemented  and  evaluated. 

For  simplicity,  waltz  closed  changes  (CCL  and  CCR)  in 
sagittal  plane  are  converted  into  a  sequence  of  motions  of 
an  LIPM  (or  two).  Several  assumptions  are  made  on  the 
controller  ((6)),  the  connection  (Fig.  6),  and  the  interaction 
(Section  III-B).  By  comparing  model-generated  motions  with 
human  dancers’  real  motions,  the  validities  of  the  model 
and  the  assumptions  are  supported.  For  the  dance  partner 
robot,  since  the  LIPM  model  can  preserve  the  basic  features 
in  human’s  body  dynamics  while  offering  several  additional 
advantages,  it  is  an  appropriate  robot  model  for  implemen¬ 
tation. 


However,  our  analysis  are  still  far  from  complete:  due 
to  the  difficulties  in  measuring  the  interaction  force  f(t) 
(given  in  (7)  and  (8))  and  assuming  the  desired  velocities,  the 
analysis  in  Section  III  is  rather  preliminary.  In  addition,  the 
presented  discussions  only  involve  the  closed  changes,  while 
more  dance  steps  with  body  rotations  are  not  addressed. 
These  issues  need  to  be  considered  in  our  future  work. 
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